% declare all the global variables
global surfaces currentSurface;
global u1 v1 u2 v2;
global f point1 point2 geodesic;
global dd eu1 eu2 ev1 ev2 pb tb;
global obj;

% set up initial data
Ps = loadSurfacePoints();
s1 = {@(u,v)evalBezierSurfaceX(Ps{1},3,3,u,v), @(u,v)evalBezierSurfaceY(Ps{1},3,3,u,v), @(u,v)evalBezierSurfaceZ(Ps{1},3,3,u,v)};
s2 = {@(u,v)evalBezierSurfaceX(Ps{2},3,3,u,v), @(u,v)evalBezierSurfaceY(Ps{2},3,3,u,v), @(u,v)evalBezierSurfaceZ(Ps{2},3,3,u,v)};
s3 = {@(u,v)evalBezierSurfaceX(Ps{3},3,3,u,v), @(u,v)evalBezierSurfaceY(Ps{3},3,3,u,v), @(u,v)evalBezierSurfaceZ(Ps{3},3,3,u,v)};
s4 = {@(u,v)evalBezierSurfaceX(Ps{4},3,3,u,v), @(u,v)evalBezierSurfaceY(Ps{4},3,3,u,v), @(u,v)evalBezierSurfaceZ(Ps{4},3,3,u,v)};
s5 = {@(u,v)evalCubicLagrangeSurfaceX(Ps{3},u,v), @(u,v)evalCubicLagrangeSurfaceY(Ps{3},u,v), @(u,v)evalCubicLagrangeSurfaceZ(Ps{3},u,v)};
s6 = {@(u,v)evalCubicLagrangeSurfaceX(Ps{4},u,v), @(u,v)evalCubicLagrangeSurfaceY(Ps{4},u,v), @(u,v)evalCubicLagrangeSurfaceZ(Ps{4},u,v)};
surfaces = {s1, s2, s3, s4, s5, s6};
currentSurface = s1;
u1 = 0.1;
v1 = 0.1;
u2 = 0.1;
v2 = 0.3;
obj = @geodesicDistance;

% generate the figure
f = figure;

% make the drop down menu
dd = uicontrol(f, 'Style','popupmenu');
dd.String = {'Surface1', 'Surface2', 'Surface3', 'Surface4', 'Surface5', 'Surface6'};
dd.Position(3) = 100;
dd.Callback = @pickSurface;

% make the point fields
eu1 = uicontrol(f, 'Style', 'edit', 'String', string(u1), 'Position', [430, 50, 50, 20]);
eu1.Callback = @updateX1;
eu2 = uicontrol(f, 'Style', 'edit', 'String', string(u2), 'Position', [430, 20, 50, 20]);
eu2.Callback = @updateX2;
ev1 = uicontrol(f, 'Style', 'edit', 'String', string(v1), 'Position', [490, 50, 50, 20]);
ev1.Callback = @updateY1;
ev2 = uicontrol(f, 'Style', 'edit', 'String', string(v2), 'Position', [490, 20, 50, 20]);
ev2.Callback = @updateY2;

t1 = uicontrol(f, 'Style', 'text', 'String', 'P1:', 'Position', [410, 50, 20, 20]);
t2 = uicontrol(f, 'Style', 'text', 'String', 'P2:', 'Position', [410, 20, 20, 20]);

% make the button for calculating the geodesic
pb = uicontrol(f,'Style','pushbutton','String','Calculate Geodesic','Position',[130 20 100 20]);
pb.Callback = @calculateGeodesic;

% make the button for switching distance calculations
tb = uicontrol(f,'Style','togglebutton','String','Subdivide Distance','Position',[250 20 100 20]);
tb.Callback = @switchDistanceFunction;

% plot the default surface and points
fsurf(currentSurface{1},currentSurface{2},currentSurface{3},[0,1]);
hold on;
point1 = plot3(currentSurface{1}(u1,v1), currentSurface{2}(u1,v1), currentSurface{3}(u1, v1),'g*');
point2 = plot3(currentSurface{1}(u2,v2), currentSurface{2}(u2,v2), currentSurface{3}(u2, v2),'g*');
hold off;

% function definitions
% UI functions
function pickSurface(src, ~)
    global surfaces currentSurface u1 v1 u2 v2 point1 point2;
    currentSurface = surfaces{src.Value};
    fsurf(currentSurface{1}, currentSurface{2}, currentSurface{3},[0,1]);
    hold on;
    point1 = plot3(currentSurface{1}(u1,v1),currentSurface{2}(u1,v1),currentSurface{3}(u1,v1),'g*');
    point2 = plot3(currentSurface{1}(u2,v2),currentSurface{2}(u2,v2),currentSurface{3}(u2,v2),'g*');
    hold off;
end

function updateX1(src, ~)
    global u1 v1 currentSurface point1 geodesic;
    u1 = str2double(src.String);
    delete(point1);
    delete(geodesic);
    hold on;
    point1 = plot3(currentSurface{1}(u1,v1), currentSurface{2}(u1,v1), currentSurface{3}(u1,v1), 'g*');
    hold off;
end

function updateX2(src, ~)
    global u2 v2 currentSurface point2 geodesic;
    u2 = str2double(src.String);
    delete(point2);
    delete(geodesic);
    hold on;
    point2 = plot3(currentSurface{1}(u2,v2), currentSurface{2}(u2,v2), currentSurface{3}(u2,v2), 'g*');
    hold off;
end

function updateY1(src, ~)
    global u1 v1 currentSurface point1 geodesic;
    v1 = str2double(src.String);
    delete(point1);
    delete(geodesic);
    hold on;
    point1 = plot3(currentSurface{1}(u1,v1), currentSurface{2}(u1,v1), currentSurface{3}(u1,v1), 'g*');
    hold off;
end

function updateY2(src, ~)
    global u2 v2 currentSurface point2 geodesic;
    v2 = str2double(src.String);
    delete(point2);
    delete(geodesic);
    hold on;
    point2 = plot3(currentSurface{1}(u2,v2), currentSurface{2}(u2,v2), currentSurface{3}(u2,v2), 'g*');
    hold off;
end

function switchDistanceFunction(src, ~)
    global obj;
    if src.Value == src.Max
        obj = @geodesicDistance2;
    elseif src.Value == src.Min
        obj = @geodesicDistance;
    end
end

function calculateGeodesic(~, ~)
    % the plot will be an approximation with stright lines
    global geodesic u1 v1 u2 v2 currentSurface obj;
    delete(geodesic);
    deltaU = (u2 - u1) / 10;
    deltaV = (v2 - v1) / 10;
    us = zeros(9,1);
    vs = zeros(9,1);
    for i=0:8
        us(i+1) = u1 + i * deltaU;
        vs(i+1) = v1 + i * deltaV;
    end
    minFunc = @(uvs)obj(currentSurface,uvs);
    uv0 = [us vs];
    options = optimoptions('fminunc','MaxFunctionEvaluations',10000);
    uvs = fminunc(minFunc,uv0,options);
    uvs = [u1 v1; uvs; u2 v2];
    xs = zeros(11,1);
    ys = zeros(11,1);
    zs = zeros(11,1);
    for i=0:10
        xs(i+1) = currentSurface{1}(uvs(i+1,1),uvs(i+1,2));
        ys(i+1) = currentSurface{2}(uvs(i+1,1),uvs(i+1,2));
        zs(i+1) = currentSurface{3}(uvs(i+1,1),uvs(i+1,2));
    end
    hold on;
    geodesic = plot3(xs,ys,zs,'go-');
    hold off;
end

function distance = geodesicDistance(surface,uv)
    % we compute the Euclidian distance between the points (estimates)
    global u1 u2 v1 v2
    us = [u1; uv(:,1); u2];
    vs = [v1; uv(:,2); v2];
    xs = zeros(size(us));
    ys = zeros(size(us));
    zs = zeros(size(us));
    for i = 1:size(us)
        xs(i) = surface{1}(us(i),vs(i));
        ys(i) = surface{2}(us(i),vs(i));
        zs(i) = surface{3}(us(i),vs(i));
    end
    distance = 0;
    for i = 2:size(us)
        distance = distance + sqrt((xs(i) - xs(i-1))^2 + (ys(i) - ys(i-1))^2 + (zs(i) - zs(i-1))^2);
    end
    disp(distance);
end

function distance = geodesicDistance2(surface,uv)
    % we compute the Euclidian distance between the points (estimates)
    global u1 u2 v1 v2
    uv_n = [uv; u2 v2];
    us = [u1; zeros(10 * size(uv_n,1),1)];
    vs = [v1; zeros(10 * size(uv_n,1),1)];
    for i=1:size(uv_n(:,1))
        u = us(10 * (i-1) + 1);
        v = vs(10 * (i-1) + 1);
        deltaU = (uv_n(i,1) - u) / 10;
        deltaV = (uv_n(i,2) - v) / 10;
        for j=1:10
            us(10 * (i-1) + 1 + j) = u + j * deltaU;
            vs(10 * (i-1) + 1 + j) = v + j * deltaV;
        end
    end
    xs = zeros(size(us));
    ys = zeros(size(us));
    zs = zeros(size(us));
    for i = 1:size(us)
        xs(i) = surface{1}(us(i),vs(i));
        ys(i) = surface{2}(us(i),vs(i));
        zs(i) = surface{3}(us(i),vs(i));
    end
    distance = 0;
    for i = 2:size(us)
        distance = distance + sqrt((xs(i) - xs(i-1))^2 + (ys(i) - ys(i-1))^2 + (zs(i) - zs(i-1))^2);
    end
    disp(distance);
end

% surface functions
function P = evalBezierSurfaceX(Ps, m, n, u, v)
    P = 0;
    for i=0:m
        for j=0:n
            P = P + Ps(i+1, j+1, 1) * factorial(m) / (factorial(i) * factorial(m-i)) * (1-u)^(m-i) * u^i * factorial(n) / (factorial(j) * factorial(n-j)) * (1-v)^(n-j) * v^j;
        end
    end
end

function P = evalBezierSurfaceY(Ps, m, n, u, v)
    P = 0;
    for i=0:m
        for j=0:n
            P = P + Ps(i+1, j+1, 2) * factorial(m) / (factorial(i) * factorial(m-i)) * (1-u)^(m-i) * u^i * factorial(n) / (factorial(j) * factorial(n-j)) * (1-v)^(n-j) * v^j;
        end
    end
end

function P = evalBezierSurfaceZ(Ps, m, n, u, v)
    P = 0;
    for i=0:m
        for j=0:n
            P = P + Ps(i+1, j+1, 3) * factorial(m) / (factorial(i) * factorial(m-i)) * (1-u)^(m-i) * u^i * factorial(n) / (factorial(j) * factorial(n-j)) * (1-v)^(n-j) * v^j;
        end
    end
end

function P = evalCubicLagrangeSurfaceX(Ps, u, v)
    f = zeros(1,4);
    g = zeros(1,4);
    f(1) = -9/2 * u^3 + 9 * u^2 - 11/2 * u + 1;
    f(2) = 27/2 * u^3 - 45/2 * u^2 + 9 * u;
    f(3) = -27/2 * u^3 + 18 * u^2 - 9/2 * u;
    f(4) = 9/2 * u^3 - 9/2 * u^2 + u;
    g(1) = -9/2 * v^3 + 9 * v^2 - 11/2 * v + 1;
    g(2) = 27/2 * v^3 - 45/2 * v^2 + 9 * v;
    g(3) = -27/2 * v^3 + 18 * v^2 - 9/2 * v;
    g(4) = 9/2 * v^3 - 9/2 * v^2 + v;
    
    P = 0;
    for i=0:3
        for j=0:3
            P = P + Ps(i+1,j+1,1) * f(i+1) * g(j+1);
        end
    end
end

function P = evalCubicLagrangeSurfaceY(Ps, u, v)
    f = zeros(1,4);
    g = zeros(1,4);
    f(1) = -9/2 * u^3 + 9 * u^2 - 11/2 * u + 1;
    f(2) = 27/2 * u^3 - 45/2 * u^2 + 9 * u;
    f(3) = -27/2 * u^3 + 18 * u^2 - 9/2 * u;
    f(4) = 9/2 * u^3 - 9/2 * u^2 + u;
    g(1) = -9/2 * v^3 + 9 * v^2 - 11/2 * v + 1;
    g(2) = 27/2 * v^3 - 45/2 * v^2 + 9 * v;
    g(3) = -27/2 * v^3 + 18 * v^2 - 9/2 * v;
    g(4) = 9/2 * v^3 - 9/2 * v^2 + v;
    
    P = 0;
    for i=0:3
        for j=0:3
            P = P + Ps(i+1,j+1,2) * f(i+1) * g(j+1);
        end
    end
end

function P = evalCubicLagrangeSurfaceZ(Ps, u, v)
    f = zeros(1,4);
    g = zeros(1,4);
    f(1) = -9/2 * u^3 + 9 * u^2 - 11/2 * u + 1;
    f(2) = 27/2 * u^3 - 45/2 * u^2 + 9 * u;
    f(3) = -27/2 * u^3 + 18 * u^2 - 9/2 * u;
    f(4) = 9/2 * u^3 - 9/2 * u^2 + u;
    g(1) = -9/2 * v^3 + 9 * v^2 - 11/2 * v + 1;
    g(2) = 27/2 * v^3 - 45/2 * v^2 + 9 * v;
    g(3) = -27/2 * v^3 + 18 * v^2 - 9/2 * v;
    g(4) = 9/2 * v^3 - 9/2 * v^2 + v;
    
    P = 0;
    for i=0:3
        for j=0:3
            P = P + Ps(i+1,j+1,3) * f(i+1) * g(j+1);
        end
    end
end

function Ps = loadSurfacePoints()
    % a flat plane
    Ps1 = zeros(4,4,3);
    Ps1(1,1,:) = [-3,-3,0];
    Ps1(2,1,:) = [-1,-3,0];
    Ps1(3,1,:) = [1,-3,0];
    Ps1(4,1,:) = [3,-3,0];
    Ps1(1,2,:) = [-3,-1,0];
    Ps1(2,2,:) = [-1,-1,0];
    Ps1(3,2,:) = [1,-1,0];
    Ps1(4,2,:) = [3,-1,0];
    Ps1(1,3,:) = [-3,1,0];
    Ps1(2,3,:) = [-1,1,0];
    Ps1(3,3,:) = [1,1,0];
    Ps1(4,3,:) = [3,1,0];
    Ps1(1,4,:) = [-3,3,0];
    Ps1(2,4,:) = [-1,3,0];
    Ps1(3,4,:) = [1,3,0];
    Ps1(4,4,:) = [3,3,0];
    
    % cubic bezier surface from homework 4
    Ps2 = zeros(4,4,3);
    Ps2(1,1,:) = [0,0,0];
    Ps2(2,1,:) = [1,0,1];
    Ps2(3,1,:) = [2,0,-1];
    Ps2(4,1,:) = [3,0,0];
    Ps2(1,2,:) = [0,4,0];
    Ps2(2,2,:) = [1,4,1];
    Ps2(3,2,:) = [2,4,-1];
    Ps2(4,2,:) = [3,4,0];
    Ps2(1,3,:) = [0,6,2];
    Ps2(2,3,:) = [1,5,2];
    Ps2(3,3,:) = [2,7,2];
    Ps2(4,3,:) = [3,6,2];
    Ps2(1,4,:) = [0,6,6];
    Ps2(2,4,:) = [1,5,6];
    Ps2(3,4,:) = [2,7,6];
    Ps2(4,4,:) = [3,6,6];
    
    % a noisy surface
    Ps3 = zeros(4,4,3);
    Ps3(1,1,:) = [0 0 0];
    Ps3(2,1,:) = [0 5 10];
    Ps3(3,1,:) = [0 10 0];
    Ps3(4,1,:) = [0 15 10];
    Ps3(1,2,:) = [5 0 10];
    Ps3(2,2,:) = [5 5 0];
    Ps3(3,2,:) = [5 10 10];
    Ps3(4,2,:) = [5 15 0];
    Ps3(1,3,:) = [10 0 0];
    Ps3(2,3,:) = [10 5 10];
    Ps3(3,3,:) = [10 10 0];
    Ps3(4,3,:) = [10 15 0];
    Ps3(1,4,:) = [15 0 10];
    Ps3(2,4,:) = [15 5 0];
    Ps3(3,4,:) = [15 10 10];
    Ps3(4,4,:) = [15 15 0];
    
    % a horseshoe (but not really)
    Ps4 = zeros(4,4,3);
    Ps4(1,1,:) = [0 0 0];
    Ps4(2,1,:) = [0 1 10];
    Ps4(3,1,:) = [1 1 10];
    Ps4(4,1,:) = [1 0 0];
    Ps4(1,2,:) = [0 2 0];
    Ps4(2,2,:) = [0 3 10];
    Ps4(3,2,:) = [1 3 10];
    Ps4(4,2,:) = [1 2 0];
    Ps4(1,3,:) = [0 4 0];
    Ps4(2,3,:) = [0 5 10];
    Ps4(3,3,:) = [1 5 10];
    Ps4(4,3,:) = [1 4 0];
    Ps4(1,4,:) = [0 6 0];
    Ps4(2,4,:) = [0 7 10];
    Ps4(3,4,:) = [1 7 10];
    Ps4(4,4,:) = [1 6 0];
    
    Ps = {Ps1,Ps2,Ps3,Ps4};
end