//Pixel matrix calculator
//Assuming fixed mirror at phi=45 degrees
//Laser0 at theta=0, phi=0; laser1 onwards are CCW (higher theta) if viewed from top
//When mirror is at theta=0, Laser0 is deflected vertically upwards
//for this case we use the numerically established finding that,
// with -%pi/2 < (mirror_theta-laser_theta) < %pi/2
// laser spot theta = sign(mirror_theta-laser_theta) * (%pi/2) + mirror_theta
// laser spot phi = (%pi/2) - abs(mirror_theta-laser_theta)

numlasers=8;//for N (even only) lasers, generated quad-crossing resolution is (2*N) theta by (N/2) phi
laserthetas=0:(2*%pi/numlasers):(2*%pi-1e-7);//laser theta locations
dtheta=(2*%pi)/(2*numlasers);//theta (and also phi) increment between pixels
thetas=((dtheta/2):(dtheta):(2*%pi))-%pi;//thetas of projected pixels
dphi=(%pi/2)/(numlasers/2);//phi increment between pixels
phis=((%pi/2-dphi/2):(-dphi):(0));
[Thetas,Phis]=meshgrid(thetas,phis);
Lasers=zeros(numlasers/2,numlasers*2);
Mthetas=zeros(numlasers/2,numlasers*2);
for(l=1:numlasers)
    for(k=1:(numlasers/2)) //rising diagonal
        j=pmodulo((l-1)*2+k-1,numlasers*2)+1;
        h=(numlasers/2)-k+1;
        Lasers(h,j)=(l-1);
        Mthetas(h,j)=pmodulo(laserthetas(l)-%pi/2+dtheta/2+(k-1)*dtheta+%pi,2*%pi)-%pi;
    end
    for(k=1:(numlasers/2)) //falling diagonal
        j=pmodulo((l-1)*2-k,numlasers*2)+1;
        h=(numlasers/2)-k+1;
        Lasers(h,j)=(l-1);
        Mthetas(h,j)=pmodulo(laserthetas(l)+%pi/2-dtheta/2-(k-1)*dtheta+%pi,2*%pi)-%pi;
    end
end

//confirmation
if(%f)
    //qs=0;
    st=zeros(numlasers/2,numlasers*2);
    sp=st;
    for(l=1:(numlasers*2))
        for(k=1:(numlasers/2))
            x=Thetas(k,l);
            y=Phis(k,l);
            lt=laserthetas(Lasers(k,l)+1);
            mt=Mthetas(k,l);
            st(k,l)=pmodulo(sign(pmodulo(mt-lt+%pi,%pi*2)-%pi)*(%pi/2)+mt+%pi,%pi*2)-%pi;
            sp(k,l)=%pi/2-abs(pmodulo(mt-lt+%pi,%pi*2)-%pi);
            //qs=qs+(st-x).^2+(sp-y).^2;
        end
    end
    //disp(qs);
end

//Thetas=Thetas/%pi*180;
//Phis=Phis/%pi*180;
Msteps=(Mthetas/(dtheta/2));//step index (positive and negative) if at step 0 mirror_theta=laser0_theta=0 and step +-2*numlasers is directly opposite; only odd steps show up in this matrix and single step is mirror increment by dtheta/2 (4 steps between adjacent lasers)
//Mthetas=Mthetas/%pi*180;

if(%f)
    //3D mesh plot
    Xs=cos(Thetas).*cos(Phis);
    Ys=sin(Thetas).*cos(Phis);
    Zs=sin(Phis);
    mesh(Xs,Ys,Zs);
end

//solving for laser and mstep given an index (i=row(phi),j=column(theta)) (1-based) in the above matrix
//if (i+j) is even
//  l=pmodulo((numlasers/4)-(i-j)/2,numlasers);
//  m=pmodulo((numlasers+1)+2*(j-1)+2*numlasers,4*numlasers)-2*numlasers;
//else (i+j) is odd
//  l=pmodulo((i+j-1)/2-(numlasers/4),numlasers);
//  m=pmodulo((-numlasers+1)+2*(j-1)+2*numlasers,4*numlasers)-2*numlasers;
if(%f)
    //validation
    [Xs,Ys]=meshgrid(1:2*numlasers,1:numlasers/2);
    evn=modulo(Xs+Ys,2)==0;
    l1=pmodulo((numlasers/4)-(Ys-Xs)/2,numlasers);
    m1=pmodulo((numlasers+1)+2*(Xs-1)+2*numlasers,4*numlasers)-2*numlasers;
    l2=pmodulo((Ys+Xs-1)/2-(numlasers/4),numlasers);
    m2=pmodulo((-numlasers+1)+2*(Xs-1)+2*numlasers,4*numlasers)-2*numlasers;
    Ls=zeros(numlasers/2,numlasers*2);
    Ms=zeros(numlasers/2,numlasers*2);
    Ls(evn)=l1(evn);
    Ls(~evn)=l2(~evn);
    Ms(evn)=m1(evn);
    Ms(~evn)=m2(~evn);
end

if(%t) //reverse mapping matrix
    Msteps0=Msteps;
    Msteps0(Msteps0<0)=Msteps0(Msteps0<0)+numlasers*4;
    Msteps0=round((Msteps0-1)/2);
    PhisRev=zeros(numlasers,2*numlasers);//basis of lasers (rows) * msteps (columns)
    ThetasRev=zeros(numlasers,2*numlasers);
    for(t=1:length(thetas))
        for(p=1:length(phis))
            m=Msteps0(p,t);
            l=Lasers(p,t);
            PhisRev(l+1,m+1)=p;
            ThetasRev(l+1,m+1)=t;
        end
    end
end


//for 0-based referencing, i->(i0+1), j->(j0+1)
//solving for laser and mstep given an index (i0=row(phi),j0=column(theta)) (0-based) in the above matrix
//if (i0+j0) is even
//  l=pmodulo((numlasers/4)-(i0-j0)/2,numlasers);
//  m=pmodulo((numlasers+1)+2*(j0)+2*numlasers,4*numlasers)-2*numlasers;
//else (i0+j0) is odd
//  l=pmodulo((i0+j0+1)/2-(numlasers/4),numlasers);
//  m=pmodulo((-numlasers+1)+2*(j0)+2*numlasers,4*numlasers)-2*numlasers;
