%The purpose of this code is to create a randomized connected network at
%equilibrium and then see how the network is affected when one of the
%points are moved. The change in network should be based on the force
%exerted on the network and should try to move so that the netowork can
%return to equilibrium.

%% Square matrix
N = 5; %Determines how big our network will be
matrix = [];
for n=0:N 
    for p=0:N 
        matrix=[matrix;n,p]; %make all x,y points in a matrix
    end
end

%Creates a gridded pattern of squares
%distance = ((X2-X1)^2-(Y2-Y1)^2)^.5
matrix_length = length(matrix); %gives the number of points 
connect = zeros(matrix_length); %makes a zero matrix based on number of points
for a= 1:matrix_length
    for b = 1:matrix_length %clean up later with a+1 to matirx length-1
        distance = ((matrix(a,1)-matrix(b,1))^2+(matrix(a,2)-matrix(b,2))^2)^.5; %distance between 2 points
        if distance == 1
            connect(a,b)= 1; %connect the points that are 1 distance apart  
        end
    end
end
figure(101)
gplot(connect, matrix,'-o')
%% Adds variation to gridded square pattern but keeps constant boundaries
[row1 col1] = find(matrix==0); %get the row and column position for all values of 0 in matrix
[row2 col2] = find(matrix==N); %get the row and column position for all values of N in matrix
rows = [row1;row2];
randmatrix = -.5 + (.5+.5)*rand(matrix_length,2); %random matrix from -.5 to .5
randmatrix(rows,:) = 0; %replace all the points that include 0 or N with 0 so that they can form the boundary
network = matrix+randmatrix; %add the random number from -.5 to .5 to matrix
figure(1)
gplot(connect, network,'-o') %plot a uniform grid
figure(2)
gplot(connect, network,'-o')
hold on 

%% Get distances between connected point in the random matrix
for a = 1:matrix_length
    for b = 1:matrix_length %clean up later with a+1 to matirx length-1
        rand_distance(a,b) = ((network(a,1)-network(b,1))^2+(network(a,2)-network(b,2))^2)^.5; %distance between 2 points
    end
end
dist = rand_distance.*connect; %Get the lengths of the connected points only


%% select a point and compare the distance from that point to others 
[x,y] = ginput(1) %select a point on the plot and get the x and y values
select_point = [x, y];
for a= 1:matrix_length
    for b = 1:matrix_length %clean up later with a+1 to matirx length-1
        if connect(a,b) == 1 %only want to compare distances of connected points
            distance(a,b) = ((network(a,1)-x)^2)+((network(a,2)-y)^2)^.5; %distance between 2 points
        else %if there is no connection at a point don't calculate the distance
            distance(a,b) = 0;
        end
    end
end

%% Select the closet point
smallest_dist = min(distance(distance>0)); %find point closest to the selected point that is not 0
[row3, col3] = find(distance == smallest_dist); %find the row and column of the smallest distance
closest_point = network(row3(1),:); %the row will be counted many times due to the symmetric matrix
%so we only choose one value of the row to get the closest point

%% select a new point to move to 
[x2,y2] = ginput(1); %select a new point and give the x and y value
new_network = network; %record the points of the new network
new_network(row3(1),:) = [x2,y2]; %replace the closest point with the new point
gplot(connect, new_network,'-o') %plot the new point movement 
hold off
figure(3)
gplot(connect, new_network,'-o')

%% Force = k*distance
k = 10; %random force constant
force = zeros(matrix_length); %Initial force will be equal to 0 because we assume the network is in equilibrium


%% Changed Network Movement Due to Moved Point
for ii = 1:100  
        
% Get distances between connected point in the newly moved matrix
for a = 1:matrix_length
    for b = 1:matrix_length %clean up later with a+1 to matirx length-1
        new_distance(a,b) = ((new_network(a,1)-new_network(b,1))^2+(new_network(a,2)-new_network(b,2))^2)^.5; %distance between 2 points
    end
end
new_dist = new_distance.*connect; %Get the lengths of the connected points only

% Compare new moved measurements
changed_length = new_dist - dist; %Difference in changed length
changed_coordinates = new_network - network; %Difference in moved coordinate
connected_row = find(changed_coordinates(:,1) ~=0); %find the rows of the connected
%points that have moved
connected_pts = zeros(1,length(connect)); %make a zero matrix the length of connect
connected_pts(connected_row) = 1; %if a point is connected then replace the 0 with a 1
connected_pts(1:N) = 0; %keep a boundary of 1:N points

x_diff = new_network(:,1) - x2; %change in x of the connected relative to the moved point
y_diff = new_network(:,2) - y2; %change in y of the connected relative to the moved point

x_diff = connected_pts'.*x_diff; %change only for connected points
x_diff = connected_pts'.*x_diff; %change only for connected points
% Find the direction vector of force

magnitude_length = (x_diff.^2 + y_diff.^2).^.5; %find magnitude of |u|
direction = [x_diff./magnitude_length,y_diff./magnitude_length]; %find the unit vector to get the x and y components for direction 
direction(isnan(direction))=0; %take out the nan and replace with 0

delta_length = changed_length.*connected_pts'; %only want the changed length of connected points
delta_length = delta_length(row3(1),:)'; %measurements are repeated because of square matrix so only get one set of values not the repeats

% Find the exerted force
F = k*delta_length; %force is only applied to connected points
F_x = F.*direction(:,1); %force in the x direction
F_y = F.*direction(:,2); %force in the y direction
F_tot = [F_x,F_y]; %force in both x and y direction

% Move points based on force
network = new_network; %replace the current network as now the old network
dis = new_distance; %replace the current distance as now the old distance

new_network_x = new_network(:,1)-F_x*.1; %connected points move in x direction of force in small increments
new_network_y = new_network(:,2)-F_y*.1; %connected points move in y direction of force in small increments
new_network = [new_network_x,new_network_y];
hold on
gplot(connect, new_network,'-o')

end


