%****************MATLAB and its Applications in Engineering (MAE)*******
%**********************Helena Calatrava Rodríguez***********************
%***************Problem set 3: MATLAB M-file Programming****************
%******************************STEP a) & b)*****************************



%-------------debug flags
two_d = 0; %flag to study the two_d scenario
three_d = 1; %flag to study the three_d scenario
track = 0; %flag to study the drone movement
stepA = 0;
stepB = 1;

%-------------init
close all;
N = 4;
delay = 0.1;

%-------------In STEP B we introduce a gaussian noise component
deviation = 10; %10 meters deviation


%-----[1]--------MATRIX P: position of the N transmitters
%transmitters are located in a 5Km by 5Km area but between 2Km and 5Km in altitude
xTransmitter = rand(N,1)*5000;
yTransmitter = rand(N,1)*5000;
zTransmitter = rand(N,1)*[5000-2000]+2000;
%each column includes the (x, y, z) coordinates
P = [xTransmitter'; yTransmitter'; zTransmitter'];
%draw each transmitter position
for n=1:N
    row = P(:,n);
    if two_d
        plot(row(1), row(2), 'r*');
        hold on
    elseif three_d
        scatter3(row(1), row(2), row(3));
        hold on
    end
end

%------[2]-------DRONE MOVEMENT
%the drone can't ever exit the volume (space of 1 km due to its trajectory)
%x and y go from 1 km to 4 km
%z goes from 0 to 2 km (no restriction because trajectory is horizontal)

%x, y and z drone init positions
x = rand()*(4000-1000)+1000;
y = rand()*(4000-1000)+1000;
z = rand()*2000;
DronePos = [x, y, z];

%angle random directions
angle1 = rand()*2*pi;
angle2 = angle1-pi/2;
angle3= angle1+pi;
angle4=angle2;

%We will draw 10 cicles
% ___     ___     ___
%| 1 | 2 | 3 | 4 |...| 10|
%|   |___|   |___|   |___|
%total iterations = 21 (10 moves of 100 m + 11 moves of 1000 m)
iteTotal = 21;
distOneSec = 25; %25m/s*s = 25 meters
angleVect = [angle1 angle2 angle3 angle2];

%Loops to compute the drone movement
for i = 1:iteTotal
    if mod(i,2) == 0
        coef = 100;
        ite = 4;
    else
        coef = 1000;
        ite = 40;
    end
    angle = angleVect(mod(i-1,4)+1);
    for j = 1:ite
        index = length(x);
        xCoord = distOneSec*cos(angle) + x(index);
        x = [x  xCoord];
        yCoord = distOneSec*sin(angle) + y(index);
        y = [y yCoord];
        if track
            plot(xCoord, yCoord, 'r*');
            hold on
        end
    end
end
if track
    axis square
    axis off
end

%Matrix Drone: matrix with the Drone Positions
%Each row corresponds to the [x y z] coordinates in the moment t secs
Drone = [x' y' repmat(z, size(x,2), 1)];
Drone = Drone';

%-----[3]--------Generation of Matrix D
%----------------Estimate the Drone movement
D = [];
drone_est = [];
T = size(Drone,2);
for t=1:T
    currentDistance = [];
    DroneCurrentPos = Drone(:,t);

    if two_d
        plot(DroneCurrentPos(1), DroneCurrentPos(2), 'b*');
        hold on
    elseif three_d
        scatter3(DroneCurrentPos(1), DroneCurrentPos(2), DroneCurrentPos(3), 10, 2);
        hold on
    end
    pause(delay);

    %We calculate the distance for every transmitter at every second
    for j = 1:N
        %use rand: Uniformly distributed pseudorandom numbers

        if stepA
            noiseComponent = 0;
        elseif stepB
            noiseComponent = deviation*rand;
        end
        newDistance = norm(DroneCurrentPos-P(:,j)) + noiseComponent;
        currentDistance = [currentDistance newDistance];
    end
    D = [D; currentDistance];

    n = 1:N;
    %anonymous function
    F = @(x,y,z) (x-P(1,n)).^2+(y-P(2,n)).^2+(z-P(3,n)).^2-D(t,n).^2;
    %we want only one variable to get rid of an error (double type error)
    F2 = @(u)F(u(1),u(2),u(3));
    %the first point to check will be [0 0 0]
    x0 = [0 0 0];
    if stepA
        %we use the fsolve function for stepA
        drone_est(:,t) = round(fsolve(F2 ,x0))';
    elseif stepB
        %we use the lsqnonlin function for stepB
        drone_est(:,t) = round(lsqnonlin(F2 ,x0))';
    end
    estim = drone_est(:,t);
    if two_d
        plot(estim(1), estim(2), 'm*');
        hold on
    elseif three_d
        scatter3(estim(1), estim(2), estim(3), 10, 1);
        hold on
    end
    pause(delay);
end