import java.io.File;

import lejos.hardware.Button;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.SampleProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.utility.Delay;
import lejos.hardware.Sound;

public class Deplacement {
	// DEFAULT SPEED
	public static final int defaultLinearSpeed = 100;
	public static final int defaultRotationalSpeed = 100;

	// MOTORS
	public static final EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.B);
	public static final EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.C);

	// PILOT
	// Initialize motor parameters
	private static final float wheelDiameter = 56f; // mm diameter wheel
	private static final float trackWidth = 123f; // mm distance bw wheels
	public static final DifferentialPilot pilote = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor,
			rightMotor);

	// SENSORS
	private static final float ultrasoundThreshold = (float) 0.10; // in m
	private static final EV3UltrasonicSensor ultrasoundSensor = new EV3UltrasonicSensor(SensorPort.S1);
	private static final EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S3);
	private static final EV3GyroSensor gyroSensor = new EV3GyroSensor(SensorPort.S4);

	Point MaPosition;
	Point Arrive;
	//PositionClient robot = new PositionClient("EV3",4);
	MulticastClient robot = new MulticastClient();
	float[] maPos;
	//Distance d;
	int K1; int K2; int K3;
	int vmax=320;
	public static void main(String[] args) throws Exception {
		Lecture_de_Fichier coord = new  Lecture_de_Fichier("Testsami.txt",8);
	}

	Deplacement(double[][] parcours) {
		// le tableau parcours renvoie les points à parcourir en ordre
		int compt = 0;
		double x, y, theta, xd, yd, ex, ey, theta_d, theta_e, vg, vd, vref, w,d;
		while (compt < parcours.length) {
			//partie recuperz la position du robot
			maPos = robot.getCoord_mobile();
			x = (double)maPos[0];
			y = (double)maPos[1];
			theta =(double)maPos[3];
			xd = parcours[compt][0];
			yd = parcours[compt][1];
			theta_d=(float)( Math.atan2(yd-y, xd-x) * 180 / Math.PI);
			theta_e=(theta_d-theta);
			MaPosition.setX(x);MaPosition.setY(y);
			Arrive.setX(xd);Arrive.setY(yd);
			d=Math.sqrt((x - xd)*(x - xd)+(y - yd)*(y - yd));
			if(x <5000.0 && y<5000.0) {
			while (d>50) { //Math.abs(x - xd) > 50000 | Math.abs(y - yd) > 50000) { //distance en millimetre
				//d=new Distance(MaPosition,Arrive);
				//pilote.travel(d.distance*1000);
				vref=(d/(K1+d))*vmax*Math.cos(theta_e);
				w=(vref/d)*Math.sin(theta_e);

				// a verifier le 89 ?
				vg=0.5*(2*vref-89*w)*100;
				vd=0.5*(2*vref+89*w)*100;
		    	leftMotor . setSpeed ((int)vg) ;
		    	rightMotor . setSpeed ((int)vd) ;

				maPos = robot.getCoord_mobile();
				x = maPos[0]; //X
				y = maPos[1]; //Y
				theta = maPos[3];
				d=Math.sqrt((x - xd)*(x - xd)+(y - yd)*(y - yd));
				theta_d=(float)( Math.atan2(yd-y, xd-x) * 180 / Math.PI);
				theta_e=(theta_d-theta);
				//MaPosition.setX(x);MaPosition.setY(y);
			} // Fin si le robot atteint le point
		}
			System.out.println("Le point est atteint");
			compt++;
		} //Fin boucle de la liste
	} // Fin methode Deplacement

}