%% 采用最小二乘法和拉格朗日成数法约束边界条件来生成五次多项式。
% （x,y）为一组坐标值
% sample_num为坐标数量
function[coeff]=quinticSplineMA(x,y,sample_num,vel,acc)
coeffnum=6;%11次样条就是12个系数
rank=coeffnum+6;%加6是指有6个约束
coeff=zeros(rank,1);
% if m~=1 && n~= 1
%     disp("enter a vector!!!");
%     return;
% else
%     [i,j]=size(x);
%     i=max([i,j]);
%     if i ~=n
%         disp("the dimensions of x and y are different\n");
%         return;
%     end
% end

% y(x)=b0+b1*x^1+b2*x^2+b3*x^3+b4*x^4+b5*x^5

%% 赋值矩阵C
C=zeros(rank);
for i=1:coeffnum
    for j=1:coeffnum
        for m=1:sample_num
            C(i,j)=C(i,j)+x(m)^(i+j-2);
        end
        C(i,j)=C(i,j).*2;
    end
end

for i=1:coeffnum
    C(coeffnum+1,i)=x(1)^(i-1);
%     C(i,7)=x(1)^(i-1);
    C(coeffnum+2,i)=x(25)^(i-1);
%     C(i,8)=x(25)^(i-1);
end

for i=1:coeffnum-1
%     C(i+1,9)=i*x(1)^(i-1);
    C(coeffnum+3,i+1)=i*x(1)^(i-1);
%     C(i+1,10)=i*x(25)^(i-1);
    C(coeffnum+4,i+1)=i*x(25)^(i-1);
end

for i=1:coeffnum-2
    C(coeffnum+5,i+2)=i*(i+1)*x(1)^(i-1);
    C(coeffnum+6,i+2)=i*(i+1)*x(25)^(i-1);
end

% for i=1:coeffnum-3
%     C(coeffnum+7,i+3)=i*(i+1)*(i+2)*x(1)^(i-1);
%     C(coeffnum+8,i+3)=i*(i+1)*(i+2)*x(25)^(i-1);
% end

for i=coeffnum+1:rank
    for j=1:coeffnum
        C(j,i)=C(i,j);
    end
end

%% 赋值矩阵B
B=zeros(rank,1);
for i=1:coeffnum
    for m=1:sample_num
        B(i,1)=B(i,1)+y(m)*x(m)^(i-1);
    end
    B(i,1)=B(i,1)*2;
end

B(coeffnum+1,1)=y(1);
B(coeffnum+2,1)=y(25);
B(coeffnum+3,1)=vel(1);
B(coeffnum+4,1)=vel(2);
B(coeffnum+5,1)=acc(1);
B(coeffnum+6,1)=acc(2);
% B(coeffnum+7,1)=jerk(1);
% B(coeffnum+8,1)=jerk(2);

coeff=C\B;
coeff=coeff(1:coeffnum);