function [X covarianceOut, err]=fitSinusLinearOptimized(datax,datay,puls)
nbdata=max(size(datax));
A=zeros(nbdata,3);
B=zeros(nbdata,1);

err=zeros(nbdata,1);
var=zeros(nbdata,1);

ATA=zeros(3,3);
ATB=zeros(3,1);
ATAa=0;
ATAb=0;
ATAc=0;
ATAd=0;
ATAe=0;
ATAf=0;
ATBa=0;
ATBb=0;
ATBc=0;

cdx=zeros(nbdata,1);
sdx=zeros(nbdata,1);
cdx2=zeros(nbdata,1);
sdx2=zeros(nbdata,1);
cdxsdx=zeros(nbdata,1);

for i=1:nbdata
    x=datax(i);
    y=datay(i);
    B(i)=y;
    dx=puls*x;
    cx=cos(dx);
    sx=sin(dx);
    cx2=cx*cx;
    sx2=sx*sx;
    cxsx=cx*sx;
    cdx(i)=cx;
    sdx(i)=sx;
    cdx2(i)=cx2;
    sdx2(i)=sx2;
    cdxsdx(i)=cxsx;
    
    ATAa=ATAa+1;
    ATAb=ATAb+cx;
    ATAc=ATAc+sx;
    ATAd=ATAd+cx2;
    ATAe=ATAe+cxsx;
    ATAf=ATAf+sx2;
    
    ATBa=ATBa+y;
    ATBb=ATBb+cx*y;
    ATBc=ATBc+sx*y;
end
%complete symetric matrices

%ATA=[ATAa , ATAb , ATAc ; ATAb , ATAd , ATAe ; ATAc , ATAe , ATAf];
%ATB=[ATBa , ATBb , ATBc ]';
%leastSquaresSolution=inv(ATA)* ATB
%a=leastSquaresSolution(1);
%b1=leastSquaresSolution(2);
%b2=leastSquaresSolution(3);

determinant =- ATAf*ATAb^2 + 2*ATAb*ATAc*ATAe - ATAd*ATAc^2 - ATAa*ATAe^2 + ATAa*ATAd*ATAf;
ATAinva= (ATAd*ATAf - ATAe^2     )/determinant;
ATAinvb= (ATAc*ATAe - ATAb*ATAf  )/determinant;
ATAinvc= (ATAb*ATAe - ATAc*ATAd  )/determinant;
ATAinvd= (ATAa*ATAf - ATAc^2     )/determinant;
ATAinve= (ATAb*ATAc - ATAa*ATAe  )/determinant;
ATAinvf= (ATAa*ATAd - ATAb^2     )/determinant;

ATAinv=[ ATAinva ATAinvb ATAinvc; ATAinvb ATAinvd ATAinve; ATAinvc ATAinve ATAinvf];

a =ATAinva * ATBa + ATAinvb * ATBb + ATAinvc * ATBc;
b1=ATAinvb * ATBa + ATAinvd * ATBb + ATAinve * ATBc;
b2=ATAinvc * ATBa + ATAinve * ATBb + ATAinvf * ATBc;
Xa=[a b1 b2]
%compute b & c
b=sqrt(b1*b1+b2*b2);
c=wrapTo2Pi(atan2(b1,b2)); %bring c between 0 and 2pi

X=[a b c ];

%compute MSE and variance....
ATVarAa=0;
ATVarAb=0;
ATVarAc=0;
ATVarAd=0;
ATVarAe=0;
ATVarAf=0;

for i=1:nbdata
    er=  1 * a + cdx(i,1) * b1 + sdx(i,1) * b2;
    vari= er*er;
    err(i)=er;
    var(i)=vari;
    ATVarAa= ATVarAa + vari;
    ATVarAb= ATVarAb + cdx(i,1)  * vari;
    ATVarAc= ATVarAc + sdx(i,1)  * vari;
    ATVarAd= ATVarAd + cdx2(i,1) * vari;
    ATVarAe= ATVarAe + cdx(i,1)*sdx(i,1)  * vari;
    ATVarAf= ATVarAf + sdx2(i,1) * vari;
    
end
ATVarA=[ ATVarAa ATVarAb ATVarAc; ATVarAb ATVarAd ATVarAe; ATVarAc ATVarAe ATVarAf];
covarianceOutIntern=ATAinv'*ATVarA*ATAinv;
JJ=[1, 0 , 0;
    0,  b1/(b1^2 + b2^2)^(1/2), b2/(b1^2 + b2^2)^(1/2);
    0, 1/(b2*(b1^2/b2^2 + 1)), -b1/(b2^2*(b1^2/b2^2 + 1))];
covarianceOut=JJ*covarianceOutIntern*JJ';
end