close all; clear; clc;

Alpha = [0, 0, 0, 0];
A = [0, 1, 1.5, 0];
D = [0, 0, 0, 0];
Theta = [0, 0, 0, 0];
Sigma = [1, 1, 0, 1];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;
DH_para.Sigma = Sigma;

tn = 10;
dt = 0.1;
t = 0:dt:tn;

q0 = [0, 0, 0, 0];

num = length(t);

q1_1 = zeros(num, 1);
q2_1 = zeros(num, 1);
q3_1 = zeros(num, 1);
q4_1 = zeros(num, 1);

q1_2 = zeros(num, 1);
q2_2 = zeros(num, 1);
q3_2 = zeros(num, 1);
q4_2 = zeros(num, 1);

pxs = 0.51; pys = -2.0; pzs= 0;
pxn = 0.51; pyn = 2.0; pzn = 0;
T0 = troty(0);

for i = 1:length(t)
    px = pxs + (pxn - pxs) * t(i) / tn;
    py = pys + (pyn - pys) * t(i) / tn;
    pz = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px, py, pz);
    
    if i == 1
        q0 = q0;
    else
        q0 = [q1_1(i - 1), q2_1(i - 1),q3_1(i - 1), q4_1(i - 1)];
    end
    
    qn = ikine(T, q0, DH_para, 1);
    q1_1(i) = qn(1);
    q2_1(i) = qn(2);
    q3_1(i) = qn(3);
    q4_1(i) = qn(4);

    if i == 1
        q0 = q0;
    else
        q0 = [q1_2(i - 1), q2_2(i - 1), q3_2(i - 1), q4_2(i - 1)];
    end
    qn = ikine(T, q0, DH_para, 0);
    q1_2(i) = qn(1) - pi * 2;
    q2_2(i) = qn(2);
    q3_2(i) = qn(3);
    q4_2(i) = qn(4);
end

qs = [q1_1, q2_1, q3_1, q4_1];
q_min_index = inline_singularity_detect(qs, DH_para);

q1 = q1_1; q1(q_min_index:end) = q1_2(q_min_index:end);
q2 = q2_1; q2(q_min_index:end) = q2_2(q_min_index:end);
q3 = q3_1; q3(q_min_index:end) = q3_2(q_min_index:end);
q4 = q4_1; q4(q_min_index:end) = q4_2(q_min_index:end);

interpo_num = 20;
q1_new = interpolate(q1, q_min_index, interpo_num);

for i = 1:length(t)
    px_desired(i) = pxs + (pxn - pxs) * t(i) / tn;
    py_desired(i) = pys + (pyn - pys) * t(i) / tn;
    pz_desired(i) = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px_desired(i), py_desired(i), pz_desired(i)) * T0;
    if i < q_min_index-interpo_num || i > q_min_index+interpo_num
        qn = [q1(i), q2(i), q3(i), q4(i)];
    elseif i < q_min_index
        qn = ikine2(T, q0, DH_para, q1_new(i), 0);
    else
        qn = ikine2(T, q0, DH_para, q1_new(i), 1);
    end

    for j = 1:4
        if j == 3
            continue
        end
        if qn(j) - q0(j) > pi
            qn(j) = qn(j) - 2 * pi;
        elseif qn(j) - q0(j) < -pi
            qn(j) = qn(j) + 2 * pi;
        end
    end

    q0 = qn;
    q(i, :) = qn;
    
    T_actual = fkine(qn, DH_para);
    px_actual(i) = T_actual(1, 4);
    py_actual(i) = T_actual(2, 4);
    pz_actual(i) = T_actual(3, 4);
end

dq = diff(q) / dt;
ddq = diff(dq) / dt;

figure(1)
for i = 1:4
subplot(2, 2, i);
plot(t, q(:, i));
xlabel("t (s)");
ylabel("q" + i + " (rad)");
end

figure(2)
for i = 1:4
subplot(2, 2, i)
plot(t(1:end-1), dq(:, i));
xlabel("t (s)");
ylabel("dq" + i + " (rad/s)");
end

figure(3)
for i = 1:4
subplot(2, 2, i)
plot(t(1:end-2), ddq(:, i));
xlabel("t (s)");
ylabel("ddq" + i + " (rad/s^2)");
end

figure(4);
subplot(3, 1, 1);
plot(t, px_desired);
hold on;
plot(t, px_actual, '--');
xlabel("t (s)");
ylabel("px" + " (m)");
legend("desired", "actual");
ylim([0.4, 0.6]);

subplot(3, 1, 2);
plot(t, py_desired);
hold on;
plot(t, py_actual, '--');
xlabel("t (s)");
ylabel("py" + " (m)");
legend("desired", "actual");

subplot(3, 1, 3);
plot(t, pz_desired);
hold on;
plot(t, pz_actual, '--');
xlabel("t (s)");
ylabel("pz" + " (m)");
legend("desired", "actual");

q0_timeseries = timeseries([q1_1, q2_1, q3_1, q4_1], t);
q1_timeseries = timeseries(q, t);