/*
	Horizon artificiel ()
 
	pour  ESP32 Wroom + afficheur 3.5" TFT 480x320
 
	par Silicium628
 
*/
 
 
/* ***************   IMPORTANT CONCERNANT L'AFFICHAGE TFT**********************************************
 
dans le fichier User_Setup.h (  dans ~/Arduino/libraries/TFT_eSPI/  )
 
// ILI9486_DRIVER ou ILI9488_DRIVER si couleurs affichées merdiques
// pour les afficheurs à CI bleu  -> ILI9486_DRIVER
// pour les afficheurs à CI rouge -> ILI9488_DRIVER
****************************************************************************
 
les lignes qui suivent ne sont qu'un commentaire pour vous indiquer la config à utiliser
placée ici, elle ne sont pas fonctionnelles
Il FAUT modifier le fichier User_Setup.h installé par le système Arduino dans ~/Arduino/libraries/TFT_eSPI/
 
// ESP32 pins used for the parallel interface TFT
#define TFT_CS   27  // Chip select control pin
#define TFT_DC   14  // Data Command control pin - must use a pin in the range 0-31
#define TFT_RST  26  // Reset pin
 
#define TFT_WR   12  // Write strobe control pin - must use a pin in the range 0-31
#define TFT_RD   13
 
#define TFT_D0   16  // Must use pins in the range 0-31 for the data bus
#define TFT_D1   4  // so a single register write sets/clears all bits
#define TFT_D2   2  // 23
#define TFT_D3   22
#define TFT_D4   21
#define TFT_D5   15 // 19
#define TFT_D6   25 // 18
#define TFT_D7   17
 
**************************************************************************** */
 
 
 
String version="1.0";
 
uint8_t fond_blanc = 0;
 
 
#include <stdint.h>
#include <TFT_eSPI.h> // Hardware-specific library
#include "SPI.h"
#include "Free_Fonts.h"
 
#include "FS.h"
#include "SD.h"
 
TFT_eSPI TFT480 = TFT_eSPI(); // Configurer le fichier User_Setup.h de la bibliothèque TFT480_eSPI au préalable
 
//#include <WiFi.h> // Pour un serveur WiFi 
//#include "ESPAsyncWebServer.h"
 
#include "Wire.h"
#include <MPU6050_light.h>
/*
un scan du bus i2c doit donner ceci (dans le moniteur série):
	Scanning...
	I2C device found at address 0x68
	done
*/
 
 
const char* ssid = "PFD_srv"; 
const char* password = "72r4TsJ28";
 
//AsyncWebServer server(80); // Create AsyncWebServer object on port 80
 
String argument_recu1;
String argument_recu2;
String argument_recu3;
 
// =====================================================================
//mémorisation dex pixels deux lignes H et de deux lignes V
//ce qui permet d'afficher un rectangle mobile sur l'image sans l'abimer
 
uint16_t data_L1[480]; // pixels d'une ligne Horizontale
uint16_t data_L2[480]; // pixels d'une autre ligne Horizontale
uint16_t data_C1[320]; // pixels d'une ligne Verticale ('C' comme colonne)
uint16_t data_C2[320]; // pixels d'une autre ligne Verticale
 
uint16_t x_1;  // position reçu du module positionneur_XY
uint16_t x_2;  // position reçu du module positionneur_XY
uint16_t y_1;
uint16_t y_2;
 
uint16_t memo_x1;
uint16_t memo_y1; // position de la ligne
uint16_t memo_x2;
uint16_t memo_y2;
 
uint16_t memo_x_pivot;
uint16_t memo_y_pivot;
 
float AngleX;
float AngleY;
float AngleZ;
 
char var_array32[10];// 10 char + zero terminal - pour envoi par WiFi (because 2^32 -1 = 4294967295 -> 10 caractères)
 
 
// =====================================================================
 
#define _pi 3.141592653
float raddeg =_pi/180.0;
 
 
float roulis;
float memo_roulis;
 
float R_2, T_2;
float memo_R_2, memo_T_2;
 
float tangage;
float memo_tangage;
 
float cap;
 
#define NOIR   		0x0000
#define MARRON   	0x9240
#define ROUGE  		0xF800
#define ROSE		0xFBDD
#define ORANGE 		0xFBC0
#define JAUNE   	0xFFE0
#define JAUNE_PALE  0xF7F4
#define VERT   		0x07E0
#define VERT_FONCE  0x02E2
#define OLIVE		0x05A3
#define CYAN    	0x07FF
#define BLEU_CLAIR  0x455F
#define AZUR  		0x1BF9
#define BLEU   		0x001F
#define MAGENTA 	0xF81F
#define VIOLET1		0x781A
#define VIOLET_2		0xECBE
#define GRIS_TRES_CLAIR 0xDEFB
#define GRIS_CLAIR  0xA534
#define GRIS   		0x8410
#define GRIS_FONCE  0x5ACB
#define GRIS_TRES_FONCE  0x2124
#define BLANC   	0xFFFF
 
#define GRIS_AF 	0x51C5		// 0x3985
 
#define HA_CIEL		0x33FE
#define HA_SOL		0xAA81 //0xDB60
 
// Width and height of sprite
#define SPR_W 25
#define SPR_H 16
 
uint16_t couleur_txt = BLANC;
uint16_t couleur_fond = GRIS_TRES_FONCE; //GRIS_TRES_FONCE;
uint16_t couleur_fond_txt = VERT_FONCE;
uint16_t couleur_fond_gradu = HA_CIEL;
 
TFT_eSprite SPR_HA = TFT_eSprite(&TFT480);
 
TFT_eSprite SPR_10 = TFT_eSprite(&TFT480); 
TFT_eSprite SPR_10_ciel = TFT_eSprite(&TFT480);  
TFT_eSprite SPR_10_sol  = TFT_eSprite(&TFT480);  
 
//position et dimensions de l'horizon artificiel
#define HA_x0 240
#define HA_y0 160
 
#define HA_w 680
#define HA_h 24
 
MPU6050 mpu(Wire);
//unsigned long timer = 0;
 
uint8_t  flag_SDcardOk=0;
uint8_t flag_1er_passage =1;
uint8_t TEST_AFFI;
 
uint32_t compte=0;
 
 
 
float degTOrad(float angle)
{
	return (angle * M_PI / 180.0);
}
 
 
uint8_t decToBcd( int val )
{
   return (uint8_t) ((val / 10 * 16) + (val % 10));
}
 
 
uint16_t Color_To_565(uint8_t r, uint8_t g, uint8_t b)
{
	return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | ((b & 0xF8) >> 3);
}
 
 
void RGB565_to_888(uint16_t color565, uint8_t *R, uint8_t *G, uint8_t *B)
{
	*R=(color565 & 0xFFFFF800) >> 8;
	*G=(color565 & 0x7E0) >> 3;
	*B=(color565 & 0x1F) << 3 ;
}
 
 
void init_SDcard()
{
	String s1;
 
	TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
	TFT480.setTextColor(BLANC, NOIR);
	TFT480.setFreeFont(FF1);
 
	uint16_t y=0;
 
	TFT480.drawString("PRIMARY FLIGHT DISPLAY", 0, y);
	y+=20;
 
	s1="version " + version;
	TFT480.drawString(s1, 0, y);
 
	y+=40;
	TFT480.setTextColor(VERT, NOIR);
	TFT480.drawString("Init SDcard", 0, y);
	y+=20;
 
 
 	if(!SD.begin())
	{
		TFT480.drawString("Card Mount Failed", 0, y);
		delay (2000);
		TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
		return;
	}
 
 
    uint8_t cardType = SD.cardType();
 
    if(cardType == CARD_NONE)
    {
        TFT480.drawString("No SDcard", 0, y);
        delay (2000);
		TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
        return;
    }
 
    flag_SDcardOk=1;
 
    TFT480.drawString("SDcard Type: ", 0, y);
	if(cardType == CARD_SD)    {TFT480.drawString("SDSC", 150, y);}
    else if(cardType == CARD_SDHC) {TFT480.drawString("SDHC", 150, y);}
 
	y+=20;
 
	uint32_t cardSize = SD.cardSize() / (1024 * 1024);
	s1=(String)cardSize + " GB";
	TFT480.drawString("SDcard size: ", 0, y);
	TFT480.drawString(s1, 150, y);
 
   // listDir(SD, "/", 0);
 
    //Serial.printf("Total space: %lluMB\n", SD.totalBytes() / (1024 * 1024));
    //Serial.printf("Used space: %lluMB\n", SD.usedBytes() / (1024 * 1024));
 
	delay (1000);
	TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
 
}
 
 
 
/** -----------------------------------------------------------------------------------
		CAPTURE D'ECRAN vers SDcard				
/** ----------------------------------------------------------------------------------- */
 
void write_TFT_on_SDcard() // enregistre le fichier .bmp
{
 
	//TFT480.setTextColor(VERT, NOIR);
	//TFT480.drawString("CP", 450, 300);
 
	if (flag_SDcardOk==0) {return;}
 
	String s1;
	uint16_t ys=200;
	TFT480.setFreeFont(FF1);
	TFT480.setTextColor(JAUNE, NOIR);
 
	uint16_t x, y;
	uint16_t color565;
	uint16_t bmp_color;
	uint8_t R, G, B;
 
	if( ! SD.exists("/bmp/capture2.bmp"))
	{
		TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
		TFT480.setTextColor(ROUGE, NOIR);
		TFT480.drawString("NO /bmp/capture2.bmp !", 100, ys);
		delay(300);
		TFT480.fillRect(100, ys, 220, 20, NOIR); // efface
		return;
	}
 
 
	File File1 = SD.open("/bmp/capture2.bmp", FILE_WRITE); // ouverture du fichier binaire (vierge) en écriture
	if (File1)
	{
/*
Les images en couleurs réelles BMP888 utilisent 24 bits par pixel: 
Il faut 3 octets pour coder chaque pixel, en respectant l'ordre de l'alternance bleu, vert et rouge.
*/
		uint16_t bmp_offset = 138;
		File1.seek(bmp_offset);
 
 
		TFT480.setTextColor(VERT, NOIR);;	
 
		for (y=320; y>0; y--)
		{
			for (x=0; x<480; x++)
			{
				color565=TFT480.readPixel(x, y);
 
				RGB565_to_888(color565, &R, &G, &B);
 
				File1.write(B); //G
				File1.write(G); //R
				File1.write(R); //B
			}
 
			s1=(String) (y/10);
			TFT480.fillRect(450, 300, 20, 20, NOIR);
			TFT480.drawString(s1, 450, 300);// affi compte à rebour
		} 
 
		File1.close();  // referme le fichier
		TFT480.fillRect(450, 300, 20, 20, NOIR); // efface le compte à rebour
	}
}
 
/** ----------------------------------------------------------------------------------- */
 
 
 
 
void Draw_arc_elliptique(uint16_t x0, uint16_t y0, int16_t dx, int16_t dy, float alpha1, float alpha2, uint16_t couleur)
// alpha1 et alpha2 en radians
{
/*
REMARQUES :	
-cette fonction permet également de dessiner un arc de cercle (si dx=dy), voire le cercle complet
- dx et dy sont du type int (et pas uint) et peuvent êtres négafifs, ou nuls.
-alpha1 et alpha2 sont les angles (en radians) des caps des extrémités de l'arc 
*/ 
	uint16_t n;
	float i;
	float x,y;
 
	i=alpha1;
	while(i<alpha2)
	{
		x=x0+dx*cos(i);
		y=y0+dy*cos(i+M_PI/2.0);
		TFT480.drawPixel(x,y, couleur);
		i+=0.01;  // radians
	}
}
 
 
void affi_rayon2(uint16_t x0, uint16_t y0, float r1, float R_2, float angle_i, uint16_t couleur_i)
{
// trace une portion de rayon de cercle entre les distances r1 et R_2 du centre
// angle_i en degrés décimaux - sens trigo
 
	float angle = degTOrad(angle_i); 
	int16_t x1, x2;
	int16_t y1, y2;
 
	x1=x0+int16_t(r1* cos(angle));
	y1=y0-int16_t(r1* sin(angle));
 
	x2=x0+int16_t(R_2* cos(angle));
	y2=y0-int16_t(R_2* sin(angle));
 
	if ((x1>0) && (x2>0) && (y1>0) && (y2>0) && (x1<480) && (x2<480) && (y1<320) && (y2<320) )
	{
		TFT480.drawLine(x1, y1, x2, y2, couleur_i);
	}
}
 
 
void affi_pointe(uint16_t x0, uint16_t y0, uint16_t r, uint16_t dr, double angle_i, float taille, uint16_t couleur_i)
{
// trace une pointe de flèche sur un cercle de rayon r
// angle_i en degrés décimaux - sens trigo
 
	float angle = degTOrad(angle_i); 
	int16_t x1, x2, x3;
	int16_t y1, y2, y3;
 
	x1=x0+r* cos(angle); // pointe
	y1=y0-r* sin(angle); // pointe
 
	x2=x0+(r-dr)* cos(angle-taille); // base A
	y2=y0-(r-dr)* sin(angle-taille); // base A		
 
	x3=x0+(r-dr)* cos(angle+taille); // base B
	y3=y0-(r-dr)* sin(angle+taille); // base B
 
	TFT480.fillTriangle(x1, y1,  x2, y2,  x3, y3,	couleur_i);	
}
 
 
void affi_base(uint16_t x0, uint16_t y0, float r, float angle_i, float delta_angle_i, uint16_t couleur_i)
{
// trace un trait tangent sur un cercle fictif de rayon r
// angle_i en degrés décimaux, sens trigo
 
	float angle =angle_i / 57.3;  // (57.3 ~ 180/pi)
	float delta_angle = delta_angle_i / 57.3; 
	int16_t x2, x3;
	int16_t y2, y3;
 
	x2=x0+ r * cos(angle-delta_angle); // x du point A de la base du triangle
	y2=y0- r * sin(angle-delta_angle); // y	
 
	x3=x0+ r * cos(angle+delta_angle); // x du point B de la base du triangle
	y3=y0- r * sin(angle+delta_angle); // y
 
	TFT480.drawLine(x2, y2, x3, y3, couleur_i);
}
 
 
 
void init_sprites()
{
	SPR_HA.createSprite(HA_w, HA_h);
	SPR_HA.setPivot(HA_w/2, HA_h/2);
	SPR_HA.fillSprite(NOIR);  // pour test en rotation --> BLEU
	SPR_HA.fillRect(0, 0, HA_w, HA_h/2, HA_CIEL);
	SPR_HA.fillRect(0, HA_h/2, HA_w, HA_h/2, HA_SOL);
 
// sprite représentant le nombre '10' sur fond noir
	SPR_10.createSprite(SPR_W, SPR_H);
	SPR_10.setFreeFont(FF5);
	SPR_10_ciel.setTextColor(BLANC, NOIR);
	SPR_10.fillSprite(NOIR);
	SPR_10.drawString("10", 2, 2 );
	SPR_10.setPivot(SPR_W/2, SPR_H/2);  // Set pivot relative to top left corner of Sprite
 
// sprite représentant le nombre '10' sur fond bleu
	SPR_10_ciel.createSprite(SPR_W, SPR_H);
	SPR_10_ciel.setFreeFont(FF5);
	SPR_10_ciel.setTextColor(BLANC, HA_CIEL);
	SPR_10_ciel.fillSprite(HA_CIEL);
	SPR_10_ciel.setPivot(SPR_W/2, SPR_H/2);  // Set pivot relative to top left corner of Sprite
	SPR_10_ciel.drawString("10", 2, 2 );
 
 
// sprite représentant le nombre '10' sur fond marron
	SPR_10_sol.createSprite(SPR_W, SPR_H);
	SPR_10_sol.setFreeFont(FF5);
	SPR_10_ciel.setTextColor(BLANC, HA_SOL);
	SPR_10_sol.fillSprite(HA_SOL);	
	SPR_10_sol.setPivot(SPR_W/2, SPR_H/2);  // Set pivot relative to top left corner of Sprite
	SPR_10_sol.drawString("10", 2, 2 );	
 
}
 
 
 
void affi_HA(float R_in, float T_in)  // Navigation Display (le grand cercle à gauche avec les différents affichages dessus)
{
	//float angle1;
 
	int16_t x_pivot, y_pivot;
	int16_t x0= 240;
	int16_t y0= 160;
 
	x_pivot = x0 + T_in * sin(degTOrad(R_in));
	y_pivot = y0 + T_in * cos(degTOrad(R_in));
	TFT480.setPivot(x_pivot, y_pivot);
	SPR_HA.pushRotated(-R_in);	// affiche la ligne de séparation ciel/sol
 
	dessine_avion();
 
// graduations
 
	for (int n=1; n<=10; n++)
	{
		float r = 10; 
		float delta_angle = 10.0;
 
		if (n==1) {delta_angle = 30.0; r = -20;}
		if (n==2) {delta_angle = 30.0; r = -40;}
		if (n==3) {delta_angle = 10.0; r = -53;}
		if (n==4) {delta_angle = 30.0; r = -80;}
		if (n==5) {delta_angle =  7.0; r = -90;}
 
		if (n==6) {delta_angle = 30.0;  r = 20;}
		if (n==7) {delta_angle = 30.0; r = 40;}
		if (n==8) {delta_angle = 10.0; r = 53;}
		if (n==9) {delta_angle = 30.0; r = 80;}
		if (n==10){delta_angle =  7.0; r = 90;}
 
		if (r > T_in) {couleur_fond_gradu = HA_SOL;} else {couleur_fond_gradu = HA_CIEL;}
 
//TFT480.fillRect(452, 90+r, 5, 5, couleur_fond_gradu); // pour test
 
		affi_base(x0, y0, -r, memo_roulis+90.0, delta_angle, couleur_fond_gradu); // efface
		affi_base(x0, y0, -r, R_in+90.0, delta_angle, BLANC); // trace
	}
 
// affichage de l'étiquette '10' sur la graduation
 
	x_pivot = x0 - 90  * sin(degTOrad(R_2-38));
	y_pivot = y0 - 90  * cos(degTOrad(R_2-38));
 
	uint16_t couleur1;
	int16_t limite = -70;
 
	if (T_2 < limite){couleur1 = HA_SOL;} else {couleur1 = HA_CIEL;}
 
	TFT480.fillRect(memo_x_pivot-16, memo_y_pivot-12, 32, 22, couleur1); // efface
	TFT480.setPivot(x_pivot, y_pivot);
	SPR_10.pushRotated(-R_2, NOIR);
 
	//dessine_avion();
 
	memo_x_pivot = x_pivot;
	memo_y_pivot = y_pivot;
 
//void affi_pointe(uint16_t x0, uint16_t y0, uint16_t r, uint16_t dr, double angle_i, float taille, uint16_t couleur_i)
 
	affi_pointe(240, 160, 120, 12, memo_roulis+90, 0.05, HA_CIEL);
	affi_pointe(240, 160, 120, 12, R_in+90, 0.05, BLANC);
 
	float alpha;
	uint8_t z;
 
 
// graduations 10	20	30	45	60
	affi_graduation_fixe();
 
	memo_roulis = R_in;
}
 
 
 
void dessine_avion() // sous forme d'équerres horizontales noires entourées de blanc
{
// aile gauche	
	TFT480.fillRect(HA_x0-102, HA_y0-3, 60, 10, BLANC);	//H contour en blanc
	TFT480.fillRect(HA_x0-42, HA_y0-3, 10, 19, BLANC);	//V
 
	TFT480.fillRect(HA_x0-100, HA_y0-1, 60, 5, NOIR);	//H
	TFT480.fillRect(HA_x0-40, HA_y0-1, 5, 15, NOIR);		//V
 
 
// aile droite	
	TFT480.fillRect(HA_x0+28, HA_y0-3, 64, 10, BLANC);	//H contour en blanc
	TFT480.fillRect(HA_x0+28, HA_y0-3, 10, 19, BLANC);	//V
 
	TFT480.fillRect(HA_x0+30, HA_y0-1, 60, 5, NOIR);		//H
	TFT480.fillRect(HA_x0+30, HA_y0-1, 5, 15, NOIR);		//V
 
//carré blanc au centre
 
	TFT480.fillRect(HA_x0-4, HA_y0-3, 8, 2, BLANC);
	TFT480.fillRect(HA_x0-4, HA_y0-3, 2, 8, BLANC);
 
	TFT480.fillRect(HA_x0-4, HA_y0+3, 10, 2, BLANC);
	TFT480.fillRect(HA_x0+4, HA_y0-3, 2, 8, BLANC);
 
}
 
 
 
 
 
void affi_ligne1_V(uint16_t x)
{
/**	DOC: (source : "TFT_eSPI.h")
// The next functions can be used as a pair to copy screen blocks (or horizontal/vertical lines) to another location
 
// Read a block of pixels to a data buffer, buffer is 16 bit and the size must be at least w * h
void readRect(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data);
  
// Write a block of pixels to the screen which have been read by readRect()
void pushRect(int32_t x, int32_t y, int32_t w, int32_t h, uint16_t *data);
**/
 
	TFT480.pushRect(memo_x1, 0, 1, 320, data_C1); // efface la ligne en replaçant l'image
	memo_x1=x;
	TFT480.readRect(x, 0, 1, 320, data_C1); // memorisation de la ligne avant de tracer dessus
	//TFT480.drawFastVLine(x, 0, 320, ROUGE);
	TFT480.drawFastVLine(x, y_1, y_2-y_1, JAUNE);
}
 
 
 
void affi_ligne2_V(uint16_t x)
{
	TFT480.pushRect(memo_x2, 0, 1, 320, data_C2); // efface la ligne en replaçant l'image
	memo_x2=x;
	TFT480.readRect(x, 0, 1, 320, data_C2); // memorisation de la ligne avant de tracer dessus
	//TFT480.drawFastVLine(x, 0, 320, ROUGE);
	TFT480.drawFastVLine(x, y_1, y_2-y_1, JAUNE);
}
 
 
void affi_ligne1_H(uint16_t y)
{
	TFT480.pushRect(0, memo_y1, 480, 1, data_L1); // efface la ligne en replaçant l'image
	memo_y1=y;
	TFT480.readRect(0, y, 480, 1, data_L1); // memorisation de la ligne avant de tracer dessus
	//TFT480.drawFastHLine(0, y, 480, ROUGE);
	TFT480.drawFastHLine(x_1, y, x_2-x_1, JAUNE);
}
 
 
void affi_ligne2_H(uint16_t y)
{
	TFT480.pushRect(0, memo_y2, 480, 1, data_L2); // efface la ligne en replaçant l'image
	memo_y2=y;
 
	TFT480.readRect(0, y, 480, 1, data_L2); // memorisation de la ligne avant de tracer dessus
	//TFT480.drawFastHLine(0, y, 480, ROUGE);
	TFT480.drawFastHLine(x_1, y, x_2-x_1, JAUNE);
}
 
 
 
void setup()
{
 
	//Serial.begin(115200);
 
	//Wire.begin(21,22, 100000); // choisir d'autre pins pour compatibilité avec l'afficheur LCD que je n'ai pas pu déplacer
 
	uint8_t GPIO_SDA = 33;
	uint8_t GPIO_SCL = 32;
 
	Wire.begin(GPIO_SDA, GPIO_SCL, 100000); // OK (source: https://randomnerdtutorials.com/esp32-i2c-communication-arduino-ide/ )
// en conséquece câbler le MPU6050 en i2C sur les GPIO 32 et GPIO 33 de l'ESP32	(à la place de 21, 22 par défaut)
 
	//Serial.println("display.init()");
 
	TFT480.init();
 
	TFT480.setRotation(3); // 0..3 à voir, suivant disposition de l'afficheur et sa disposition
 
	TFT480.fillScreen(NOIR);
	TFT480.setTextColor(BLANC, NOIR);
 
	TFT480.setFreeFont(FF1);
	uint16_t y=0;
	TFT480.drawString("Horizon artificiel", 0, y);
	y+=20;
	String s1="version " + version;
	TFT480.drawString(s1, 0, y);
	y+=20;
 
 
	delay(300);
 
	byte status = mpu.begin();
 
	s1="MPU6050 status:" + String(status);
	TFT480.setTextColor(JAUNE, NOIR);
	TFT480.drawString(s1, 0, y);
	y+=20;
 
	////while(status!=0){ } // stop everything if could not connect to MPU6050
	TFT480.setTextColor(BLANC, NOIR); 
	s1="Calcul offsets, do not move MPU6050";
	TFT480.drawString(s1, 0, y);
	y+=20;
 
	delay(1000);
	// mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
	mpu.calcOffsets(); // gyro and acceler
 
	TFT480.setTextColor(VERT, NOIR); 
	s1="OK!\n";
	TFT480.drawString(s1, 0, y);
	y+=20;
 
	delay(1000);
 
	TFT480.setTextColor(TFT_BLUE, TFT_BLACK);
	//TFT480.setTextFont(1);
	TFT480.setCursor(0, 40, 1);
	TFT480.fillScreen(couleur_fond);
 
	//init_SDcard();
 
	init_sprites();
 
	TFT480.fillRect(0, 0, 480, 160, HA_CIEL);
	TFT480.fillRect(0, 160, 480, 320, HA_SOL);	
 
	R_2=0;
	T_2=0;
	affi_HA(R_2,T_2);
 
	x_1=0;
	y_1=0;
 
	mpu.update();
 
	tangage = 0;
	roulis  = 0;
 
	TEST_AFFI=0;
 
	for(int n=0; n<200; n++)
	{
		mpu.update(); // calme la bête !
	}
 
	///delay(1000);
	///write_TFT_on_SDcard();
 
}
 
 
void affi_graduation_fixe()
{
	int16_t x0= 240;
	int16_t y0= 160;
 
	affi_rayon2(x0, y0, 125, 140, 90, BLANC);
 
	affi_rayon2(x0, y0, 125, 130, 90-10, BLANC);
	affi_rayon2(x0, y0, 125, 130, 90-20, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90-30, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90-45, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90-60, BLANC);
 
	affi_rayon2(x0, y0, 125, 130, 90+10, BLANC);
	affi_rayon2(x0, y0, 125, 130, 90+20, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90+30, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90+45, BLANC);
	affi_rayon2(x0, y0, 125, 140, 90+60, BLANC);
 
	TFT480.setFreeFont(FF5);
	TFT480.setTextColor(BLANC, HA_CIEL);
	TFT480.drawString("30", x0+70, 23);
	TFT480.drawString("30", x0-90, 23);
 
	TFT480.drawString("60", x0+125, 75);
	TFT480.drawString("60", x0-150, 75);
}
 
 
 
void affichages()
{
 
//les lignes suivantes obligent la rotation pas à pas
 
	//if (R_2<roulis-10)	{ affi_HA(R_2, T_2); R_2+=2.5; }
	if (R_2<roulis-3)	{ affi_HA(R_2, T_2); R_2+=2; }
	else if (R_2<roulis) { affi_HA(R_2, T_2); R_2++; }
 
	//if (R_2>roulis+10)	{ affi_HA(R_2, T_2); R_2-=2.5; }
	if (R_2>roulis+3)	{ affi_HA(R_2, T_2); R_2-=2; }
	else if (R_2>roulis) { affi_HA(R_2, T_2); R_2--; }
 
 
 
	if (T_2<tangage-3) { affi_HA(R_2, T_2); T_2+=2; }
	else if (T_2<tangage) { affi_HA(R_2, T_2); T_2++; }
 
	if (T_2>tangage+3) { affi_HA(R_2, T_2); T_2-=2; }
	else if (T_2>tangage) { affi_HA(R_2, T_2); T_2--;}
 
}
 
 
 
float t=0;
float dt=2;
 
void loop()
{
 
	if(TEST_AFFI==1) // pour test des affichages:
	{
		roulis = t;
		tangage = 60.0*sin(t/134);
 
		affichages();
		t += dt; // 3*dt ...
		if ((t==120)||(t== -120)) {dt = -dt;}
		//delay(1);
	}
 
 
	else  
	{
		mpu.update();
 
		AngleX=mpu.getAngleX();
		AngleY=mpu.getAngleY();
		AngleZ=mpu.getAngleZ();
		//timer = millis();  
 
		tangage = -5.0*AngleX;
		roulis  = -1.0*AngleY;
 
		affichages();
		compte++;
		delay(1);
 
		// if (compte==100) {write_TFT_on_SDcard();}
	}
}