#include <Stepper.h>

#define RAZ 12

#define PROXY_SENSOR 0

const int mem[29] = { // 0-29cm
		621, //0
				603, //1
				594, //2
				585, //3
				576, //4
				552, //5
				525, //6
				478, //7
				417, //8
				367, //9
				335, //10
				280, //11
				267, //12
				230, //13
				210, //14
				190, //15
				173, //16
				157, //17
				144, //18
				130, //19
				120, //20
				112, //21
				100, //22
				95, //23
				85, //24
				77, //25
				70, //26
				60, //27
				58  //28
		};
unsigned long val = 0;
int res = 0; // resultat
int cnt = 0;

// Les X et Y filtrÈs de l'image prÈcÈdente
int ofx;

// Les X et Y filtrÈs
int fx;

// La cible ‡ atteindre
int nx;

// Vitesse d'Èvolution
float sx;

// damp = frictions (1 = pas de frictions)
const float damp = 0.3;

// k = konstante de vitesse du filtre de mouvement
const float k = 0.3;

/// 4460x 180°
// 2270y  360°

// initialize of the Stepper library:
Stepper tete(200, 2, 3, 4, 5);  //anneau exerieur sur leque est le laser
Stepper rouleau(200, 6, 7, 8, 9); //la ou est posé la piece

//Stepper rouleau(400, 2,3, 4,5);
// stock position (actuelle) de la pointe sur le matériau
int positionX = 0;
int positionY = 0;

// position voulue
int targetX = 0;
int targetY = 0;

// réception depuis le port série
char sInput[5];

int serInputByte = 0;

// capteur si nous sommes arrivés
boolean isArrived = false;

void setup() {
// set the motor speed
	tete.setSpeed(45);
	rouleau.setSpeed(45);

// Initialize the Serial port:
	Serial.begin(9600);
	ofx = fx = 0;
	sx = 0;
	val = 0;
	for (int i = 0; i < 10; i++) {
		val += analogRead(0);
		delayMicroseconds(360);
	}
	val = val / 10;

// raz();
}

void loop() {
	if (Serial.available() > 0) {
		char val = Serial.read();
		if ((val!='x') && (val!='y')) {
				sInput[serInputByte] = val;
				if (serInputByte < 5) {
					serInputByte++;
				} else {
					serInputByte = 0;
				Serial.println("ERROR MORE THAN 4 BYTES IN COMMUNICATION!");
			}
		} else {
			int number = atoi(sInput);
			if (val=='x') 
				targetX = number;
			else
				targetY = number;
			
			isArrived = false;
				serInputByte = 0;
				sInput[0] = 0;
				sInput[1] = 0;
				sInput[2] = 0;
				sInput[3] = 0;
				sInput[4] = 0;
		}
	}

	/// multitache pilotage des moteurs
	int xTarg = targetX - positionX;
	int yTarg = targetY - positionY;
	if (xTarg != 0) {
		if (xTarg > 0) { // avance si plus petit
			tete.step(1);
			positionX++;
		} else { /// recule si plus grand
			tete.step(-1);
			positionX--;
		}
	}
	if (yTarg != 0) {
		if (yTarg > 0) {
			rouleau.step(1);
			positionY++;
		} else {
			rouleau.step(-1);
			positionY--;
		}
	}

	/// sync avec l'ordi, ça y est je suis arrivé
	if ((xTarg == 0) && (yTarg == 0)) {
		if (isArrived != true) {

			//  Serial.println(‘o');
			readAndSendSensorData();
		}
		isArrived = true;
	}
	/////////
	if (cnt < 20) {
		cnt++;
	} else {
		val = 0;
		for (int i = 0; i < 10; i++) {
			val += analogRead(0);
			delayMicroseconds(360);
		}
		val = val / 10;
		nx = val;
		cnt = 0;
		res = getMM();

		//Serial.println(res);
		sx = 0;
	}
	sx += (nx - fx) * k;
	sx *= damp;
	fx += sx;
	ofx = fx;

	//  Serial.println(proxy); /// gimme height info
}

///1170
void readAndSendSensorData() {
	Serial.println(res);
}
int getMM() {
	int minA = 0;
	int maxB = 0;
	int minCm = 0;
	int maxCm = 0;
	for (int i = 0; i < 29; i++) {
		int t = mem[i];
		if (t < fx) {
			maxB = t;
			if (i > 0) {
				minA = mem[i - 1];
				minCm = i - 1;
				maxCm = i;
			}
			i = 29;
		}
	}
	float result = mapF(fx, minA, maxB, minCm, maxCm);
	return int(result * 10.0);
}

float mapF(float value, float istart, float istop, float ostart,float ostop) {
      return ostart + (ostop - ostart) * ((value - istart) / (istop - istart));
}