package robots;

import java.util.ArrayList;

import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class LCPRobot implements Robot {
	private Pose pose;
	static DifferentialPilot pilot;
	static UltrasonicSensor sonar;

	public LCPRobot() {

	}

	@Override
	public void move(double x) {
		pilot.travel(x);
	}

	@Override
	public void rotate(double x) {
		pilot.rotate(x);
	}

	@Override
	public ArrayList<DataPose> scann(int ini, int end, int interval) {
		ArrayList<DataPose> result = new ArrayList<DataPose>();
		int val = 0;
		double ang = 0;
		int engine_mult = 1;
		Motor.C.setSpeed(200);
		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
		Motor.C.setSpeed(80);
		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
			Motor.C.rotate(-(engine_mult * interval));
			val = sonar.getDistance();
			// ang = (j/(5*end)) * 180.0;
			ang = ini + interval;

			DataPose data = new DataPose();
			data.setDistance(val);
			data.setSensorAngle(ang);
			data.setPose(pose);
			result.add(data);
		}
		Motor.C.setSpeed(200);
		Motor.C.rotate(0);
		return result;
	}

	@Override
	public String toString() {
		return "LCP Robot";
	}

	@Override
	public void moveForward() {
		pilot.forward();

	}

	@Override
	public void moveLeft() {
		pilot.rotateLeft();
	}

	@Override
	public void moveRight() {
		pilot.rotateRight();
	}

	@Override
	public void moveBackward() {
		pilot.backward();
	}

	@Override
	public void stop() {
		pilot.stop();
	}

	@Override
	public void scann(RobotReturn r) {
		// TODO Auto-generated method stub

	}

	@Override
	public void stopScann() {
		// TODO Auto-generated method stub

	}

	@Override
	public boolean connect() {
		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
		sonar = new UltrasonicSensor(SensorPort.S1);
		pilot.setTravelSpeed(10);
		pilot.setRotateSpeed(40);
		return true;
	}

	@Override
	public void disconnect() {
	}

	@Override
	public void setPose(float x, float y, float a) {
		provider.setPose(new Pose(x, y, a));
	}

}