clc
clear all
close all

% input directly%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mflag=1;
N=180;
t1=0.0d0;
t2=17.6;
t2orig=t2;

t=t1:((t2-t1)/(2*N+1)):t2;
F=2+1*cos(2*pi*t)+2*sin(2.4*pi*t);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
% % % Input from file %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% filepath='data0.dat';
% fid=fopen(filepath,'r');
% data = textscan(fid,'%s','delimiter','\n') ;
% fclose(fid);
% data=data{1,1};
% mflag=str2double(data{1});
% N=str2double(data{2});
% t1=str2double(data{3});
% t2=str2double(data{4});
% t2orig=t2;
% t=t1:((t2-t1)/(2*N+1)):t2;
% nmnode=length(data);
% if (nmnode~=2*N+2+4)
%     disp('Input error!');
%     clear all
%     return;
% end
% F=zeros(1,2*N+2);
% for i=1:(2*N+2)
%     F(i)=str2double(data{i+4});
% end
% clear data filepath fid nmnode

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% %generate data file
% fileID = fopen('data0.dat','w');
% fprintf(fileID,'%+20.12E\n',[mflag,N,t1,t2]);
% fprintf(fileID,'%+20.12E\n',F);
% fclose(fileID);
% disp('data file written successfully');
% return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%calculating denominators
fden=sqrt(sum(F(1:(2*N+2)).^2));

if (mflag==1)
    tol=1.0d-10;
    Ts=(t2-t1)/(2*N+1);
    N0=N;

    fileID = fopen('output.dat','w');

    for N=N0:(-1):12
        %         F=F(1:(2*N+2));
        %         t=t(1:(2*N+2));
        t2=t(2*N+2);
        fprintf(fileID,'%s %+15.7E %8i\n','value of t2, N = ',t2,N);

        %    while(N>2)

        %guess2----------
        A=zeros(2*N+2,1);
        T0=(t2-t1);
        for j=1:N
            for m=2:(2*N+1)
                A(2*j-1)=A(2*j-1)+2*F(m)*cos(2*pi*j*t(m)/T0)/(2*N+1);
                A(2*j)=A(2*j)+2*F(m)*sin(2*pi*j*t(m)/T0)/(2*N+1);
            end
            A(2*j-1)=A(2*j-1)+(F(1)*cos(2*pi*j*t1/T0)+F(2*N+2)*cos(2*pi*j*t2/T0))/(2*N+1);
            A(2*j)=A(2*j)+(F(1)*sin(2*pi*j*t1/T0)+F(2*N+2)*sin(2*pi*j*t2/T0))/(2*N+1);
        end
        for m=2:(2*N+1)
            A(2*N+1)=A(2*N+1)+F(m)/(2*N+1);
        end
        A(2*N+1)=A(2*N+1)+(F(1)+F(2*N+2))/(2*(2*N+1));
        A(end)=T0;
        %----------------

        fdnorm=0.0d0;
        for i=1:2*N0+2
            sum1=0.0d0;
            for j=1:N
                sum1=sum1+A(2*j-1)*cos(2*pi*j/A(2*N+2)*(t1+(i-1)*Ts))+A(2*j)*sin(2*pi*j/A(2*N+2)*(t1+(i-1)*Ts));
            end
            sum1=sum1+A(2*N+1);
            fdnorm=fdnorm+(sum1-F(i))^2;
        end
        fdnorm=sqrt(fdnorm)/fden;
        fprintf(fileID,'%s %+15.7E\n','fdnorm = ',fdnorm);

        if (fdnorm<0.4)

            for k0=1:8

                %initialize
                Kmat=zeros(2*N+2,2*N+2);
                fmat=zeros(2*N+2,1);

                Tk=A(2*N+2);
                c0=A(2*N+1);

                %compute K, f
                for i=1:(2*N+2)
                    for j=1:N
                        Kmat(i,2*j-1)=cos(2*pi*j*(t1+(i-1)*Ts)/Tk);
                        Kmat(i,2*j)=sin(2*pi*j*(t1+(i-1)*Ts)/Tk);
                    end
                    Kmat(i,2*N+1)=1;
                    sum1=0;
                    for m=1:N
                        sum1=sum1+((2*pi*m*(t1+(i-1)*Ts))/(Tk)^2)*...
                            (A(2*m-1)*sin(2*pi*m*(t1+(i-1)*Ts)/Tk)-A(2*m)*cos(2*pi*m*(t1+(i-1)*Ts)/Tk));
                    end
                    Kmat(i,2*N+2)=sum1;
                    sum1=0;
                    for m=1:N
                        sum1=sum1+A(2*m-1)*cos(2*pi*m*(t1+(i-1)*Ts)/Tk)+A(2*m)*sin(2*pi*m*(t1+(i-1)*Ts)/Tk);
                    end
                    fmat(i)=F(i)-c0-sum1;
                end

                %fnorm=sqrt(sum(fmat.^2))/fden;
                fdnorm=0.0;
                for i=1:2*N0+2
                    sum1=0;
                    for j=1:N
                        sum1=sum1+A(2*j-1)*cos(2*pi*j/A(2*N+2)*(t1+(i-1)*Ts))+A(2*j)*sin(2*pi*j/A(2*N+2)*(t1+(i-1)*Ts));
                    end
                    sum1=sum1+A(2*N+1);
                    fdnorm=fdnorm+(sum1-F(i))^2;
                end
                fdnorm=sqrt(fdnorm)/fden;

                % check and exit
                %fprintf(fileID,'%s %2i %s %10.4E\n','iteration',k0,'; fnorm =',fdnorm);
                fprintf(fileID,'iteration %2i ; fdnorm = %10.4E\n',k0,fdnorm);
                if (fdnorm>2)
                    break;
                end
                if (fdnorm<=tol&&A(2*N+2)>0&&A(2*N+2)<=t2orig-t1+0.000001)

                    % print the full solution
                    fprintf(fileID,'%s\n','             c0               T');
                    fprintf(fileID,'%+15.7E %+15.7E\n',A(2*N+1),A(2*N+2));
                    fprintf(fileID,'%s\n','    i  Amplitude (Cos) Amplitude (Sin) Frequency (Hz)');
                    for i=1:N
                        fprintf(fileID,'%5d %+15.7E %+15.7E %+15.7E\n',i,A(2*i-1),A(2*i),i/A(2*N+2));
                    end

                    % terminate the program
                    fclose(fileID);
                    disp('output file written successfully');
                    clear all
                    return;

                else

                    %solve and continue
                    udelta=Kmat\fmat;
                    A=A+udelta;

                end

            end
        end

    end

    %     %verification
    %     for i=1:(2*N+2)
    %         Tk=A(2*N+2);
    %         c0=A(2*N+1);
    %         sum1=0;
    %         for m=1:N
    %             sum1=sum1+A(2*m-1)*cos(2*pi*m*(t1+(i-1)*Ts)/Tk)+A(2*m)*sin(2*pi*m*(t1+(i-1)*Ts)/Tk);
    %         end
    %         fmat(i)=c0+sum1;
    %     end

    fclose(fileID);
    disp('output file written successfully');

elseif (mflag==2)

    c0=F(1);
    c1=F(end);

    %initialize
    Kmat=zeros(2*N,2*N);
    fmat=zeros(2*N,1);

    %compute K, f
    for i=1:(2*N)
        for j=1:(2*N)
            Kmat(i,j)=sin(i*j*pi/(2*N+1));
        end
        fmat(i)=F(i+1)-c0-(c1-c0)*i/(2*N+1);
    end

    %solve
    A=Kmat\fmat;

    %verification
    tbar=t((2:end-1));
    fmat=(c0*(t2-tbar)+c1*(tbar-t1))/(t2-t1);
    for m=1:(2*N)
        fmat=fmat+A(m)*sin(m*pi*(tbar-t1)/(t2-t1));
    end
    fmat=[c0, fmat, c1];

    fileID = fopen('output.dat','w');
    fprintf(fileID,'%s\n','             c0               c1');
    fprintf(fileID,'%+15.7E %+15.7E\n',c0,c1);
    fprintf(fileID,'%s\n','    i  Amplitude (Sin) Frequency (Hz)     phase');
    for i=1:2*N
        fprintf(fileID,'%5d %+15.7E %+15.7E %+15.7E\n',i,A(i),i/(2.0d0*(t2-t1)),-i*pi*t1/(t2-t1));
    end
    fclose(fileID);
    disp('output file written successfully');

else
    disp('invalid flag')
    clear all
    return;
end

% printing
% disp('Solution stored in variable A')
% disp(A)

% plotting
% figure()
% hold on
% plot(t,F,'o','Linewidth',1.5)
% plot(t,fmat,'rx--','Linewidth',1.5)
% hold off
% legend({'input','output'},'Interpreter','latex','Location','best')

clear all
