% resolution_modulation_fit.m
% calculates the modulation of one radius and one segment

function [result] = resolution_modulation_fit(Bild, MPy, MPx, RR, phivon, dphi, nuestern)

if ~ (0 < phivon & phivon <= 2 * pi)
    display(['********************************* value of phivon not allowed : ', num2str(phivon), ', Cancel *************************************']);
    return
end

if ~ (0 < dphi & dphi <= 2 * pi)
    display(['********************************* value of dphi not allowed : : ', num2str(dphi), ', Cancel *************************************']);
    return
end


halfind = 0; 
phibis = phivon + dphi;
ik = round(MPy - (RR) * sin(phivon)); jk = round(MPx + (RR) * cos(phivon));

phik = atan2posit(MPy - ik, jk - MPx,halfind); phikk = phik; 

graydn = double(Bild(ik, jk)); 
RR2 = RR^2;

switch floor(phik / (pi / 2 ))
    case 0          % I. Quadrant
         ry = -1; rx = -1;
         iqu = 1;
    case 1          % II. Quadrant
      ry = 1; rx = -1;
      iqu = 2;
    case -2         % III. Quadrant
      ry = 1; rx = 1; phibis = phibis - 2 * pi; 
      iqu = 3;
    otherwise       % IV. Quadrant    
      ry = -1; rx = 1; phibis = phibis - 2 * pi; 
      iqu = 4;
 end

while phik <= phibis
    [resultnext_Q,halfind] = next_Q(ik, jk, MPx, MPy, RR2, ry, rx, iqu,halfind);
    ry = resultnext_Q(1 , 1); rx = resultnext_Q(1 , 2);
    ik = resultnext_Q(2 , 1); jk = resultnext_Q(2 , 2);
    iqu = resultnext_Q(3 , 1);
    phik = atan2posit(MPy - ik, jk - MPx, halfind); 
    phikk = [phikk , phik];
    graydn = [graydn , double(Bild(ik, jk))]; 
end %while phik <= phibis

nbase = length(phikk); 

%resultfit(phi) = AA + BBcos * cos[nuestern * phi] + BBsin * sin[nuestern * phi]

cosvect = cos(nuestern * phikk); sinvect = sin(nuestern * phikk);
const1vect = ones(1, nbase); datamatrix = [const1vect; cosvect; sinvect];
matrixA = datamatrix * datamatrix';
RSvect = datamatrix * graydn';
resgvect = matrixA \ RSvect; 
AA = resgvect(1 , 1); BBcos = resgvect(2 , 1); BBsin = resgvect(3 , 1);
amplit = sqrt(BBcos^2 + BBsin^2); contrast = amplit / AA;
result = contrast;


function [result,halfind] = next_Q(ik, jk, MPx, MPy, RR2, ry, rx, Quadrant,halfind) %nächster Punkt auf dem "digi-Kreisbogen"

r1 = (MPy - ik)^2 + (jk + rx - MPx)^2; r2 = (MPy - (ik + ry))^2 + (jk - MPx)^2;
if abs(r1 - RR2) < abs(r2 - RR2)
    jk = jk + rx;
else   
    ik = ik + ry;
end

switch Quadrant
    case 1          % I. Quadrant
        if jk < MPx 
           ry = -ry; Quadrant = 2;
        end    
    case 2          % II. Quadrant
        if ik > MPy 
           rx = -rx; Quadrant = 3; halfind = 2 * pi;
       end   
    case 3          % III. Quadrant
        if jk > MPx 
           ry = -ry; Quadrant = 4;
        end    
    otherwise       % IV. Quadrant            
        if ik < MPy 
           rx = -rx; Quadrant = 1; 
       end   
end

result = [ry, rx; ik, jk; Quadrant, 0];


function result = atan2posit(Y , X, halfind);

result = atan2(Y , X) + halfind;