//This follows same assumptions as Pixel calculation.sce

//The laser at x=1,y=0 (theta=0) is considered first laser
//Lasers at more positive theta are considered next lasers
//For the first laser, the rising mirror theta line has the quarter-split points
//  starting at (theta3,phi3) when mirror_theta==dt/2 and advancing by N*(dt,-dt) where dt=laser_dtheta/2 as mirror advances by N*dt
//  for N=0 to floor((phi3-minphi)/dt) integer
//For the first laser, the falling mirror theta line
//  starting at (-theta3,phi3) when mirror_theta==-dt/2 and advancing by N*(-dt,-dt) as mirror advances by N*-dt
//For the second laser (theta=2*dt), the rising mirror theta line
//  starting at (theta3+2*dt,phi3) when mirror_theta==(2*dt+dt/2) and advancing by N*(dt,-dt) as mirror advances by N*dt

merr=0;

function [qtp,laser,laseri,mirror_step,vres]=assignlaser(tp,numlasers,accpt)
    if(modulo(numlasers,2)~=0)
        error('Odd number of lasers not supported');
    end
    laser_dtheta=2*%pi/numlasers;
    dt=laser_dtheta/2;
    theta3=%pi/2+dt/2;
    phi3=%pi/2-dt/2;
    minphi=(%pi/2)-abs(accpt); //[rad] lowest phi of reflected spot
    vres=floor((phi3-minphi)/dt)+1;
    phis=phi3-(vres-1)*(laser_dtheta/2);//[rad] lowest phi of valid pixel
    if(tp(2)<phis)
        tp(2)=phis;
    end
    li=1:vres;
    il=flipdim(li,2);
    laser_path=[[-il,li];[(-il+1)*dt-theta3,(li-1)*dt+theta3];[phi3-([il,li]-1)*dt]];
    nqpn=round((tp-[theta3;phi3])/dt);
    if(modulo(sum(nqpn),2)==0)
        //point is on rising mirror theta line
        laser=sum(nqpn)/2;//0-indexed
        laseri=nqpn(1)-sum(nqpn)+1;//1-indexed
    else
        //point is on falling mirror theta line
        nqp2=[nqpn(1),-nqpn(2)];
        lqe=ceil(num_lasers/2)+1;
        laser=(sum(nqp2)-1)/2+lqe;//0-indexed
        laseri=nqp2(1)-sum(nqp2)-1;//-1-indexed
    end
    if(laser<0)
        laser=laser+num_lasers;
    end
    if(laser>=num_lasers)
        laser=laser-num_lasers;
    end
    if(laseri<0)
        qli=laseri+vres+1;
    else
        qli=laseri+vres;
    end
    qtp=laser_path(2:3,qli)+[1;0]*(2*dt*laser);
    mirror_step=laser*4+sign(laser_path(1,qli))*(abs(laser_path(1,qli))*2-1); //only odd steps correspond to quarter-points for even num_lasers, zero is when the first laser points directly upwards, and a single step is an increment in mirror_theta by dt/2
endfunction

return
for j=1:10

//Given a point at (t,p), first find nearest quarter-point
//This is not optimized for odd num_lasers since one of the quarter-points is 
//  the top center [0;0;1] which is excluded below to avoid overlaps
tp=rand(2,1).*[2*%pi;%pi/2-phis($)]+[0;phis($)];
[qtp,laser,laseri,mirror_step,vres]=assignlaser(tp,num_lasers,laser_acceptance_angle_h);
if(laser<0 || laser>=num_lasers)
    error('Laser assignment error theta');
end
if(abs(laseri)<1 || abs(laseri)>vres)
    tps=tp;
    param3d(cos(tps(1,:)).*cos(tps(2,:)),sin(tps(1,:)).*cos(tps(2,:)),sin(tps(2,:)));
gce().mark_mode = "on";
gce().mark_style=5;
    error('Laser assignment error phi');
end
tps=[tp,qtp];
qtps=[cos(tps(1,:)).*cos(tps(2,:));sin(tps(1,:)).*cos(tps(2,:));sin(tps(2,:))];
nm=norm(qtps(:,1)-qtps(:,2));
if(nm>merr)
    merr=nm;
end
if(%f)
param3d(cos(tps(1,:)).*cos(tps(2,:)),sin(tps(1,:)).*cos(tps(2,:)),sin(tps(2,:)));
gce().mark_mode = "on";
gce().mark_style=5;
end

end
mprintf("Actual max err with %i pts = %f\n",j,merr);

return
param3d(cos(tps(1,:)).*cos(tps(2,:)),sin(tps(1,:)).*cos(tps(2,:)),sin(tps(2,:)),"markerEdgeColor",[0.5,0.5,0]);
gce().mark_mode = "on";
gce().mark_style=5;
param3d(cos(qtps(1,:)).*cos(qtps(2,:)),sin(qtps(1,:)).*cos(qtps(2,:)),sin(qtps(2,:)),"markerEdgeColor",[0,0.5,0.5]);
gce().mark_mode = "on";
gce().mark_style=4;
