/**
Optimize type II servo gains beased on measurement noise and signal PSD. We try to minimize
\f[
\sigma^2=\int \textrm{PSD}_{ngs,ws}H_{rej}\textrm{d}\nu + \int_0^{\nu_{nyquist}} \textrm{PSF}\textrm{d}\nu
\f]
*/
static void setup_aster_servo(SIM_S *simu, ASTER_S *aster, const PARMS_S *parms){
int ndtrat=parms->skyc.ndtrat;
if(aster->gain){
dcellfree(aster->gain);
dfree(aster->res_ws);
dfree(aster->res_ngs);
}
aster->gain=cellnew(ndtrat,1);
aster->res_ws=dnew(ndtrat,1);
aster->res_ngs=dnew(ndtrat,3);
PDMAT(aster->res_ngs, pres_ngs);
for(int idtrat=0; idtrat<ndtrat; idtrat++){
int dtrat=parms->skyc.dtrats->p[idtrat];
double sigma_ngs= aster->sigman->p[idtrat]->p[0];
double sigma_tt = aster->sigman->p[idtrat]->p[1];
double sigma_ps = sigma_ngs-sigma_tt;
double sigma_focus = aster->sigman->p[idtrat]->p[2];
long nmod=parms->maos.nmod;
/*gsplit:
0: All modes use the same gain.
1: PS, TT, focus (if nmod>5) use different gains.
2: PS, TT use different gains. focus mode (if nmod>5) use PS gains.
*/
double res_ngs;/*residual error due to signal after servo rejection. */
double res_ngsn;/*residual error due to noise. */
const int servotype=parms->skyc.servo;
const int ng=parms->skyc.ngain;
aster->gain->p[idtrat]=dnew(ng,nmod);
PDMAT(aster->gain->p[idtrat], pgain);
if(parms->skyc.gsplit){
double pg_tt[ng+2];
double pg_ps[ng+2];
double pg_focus[ng+2];
if(parms->skyc.interpg){
interp_gain(pg_tt, simu->gain_tt[idtrat], simu->gain_x, sigma_tt);
interp_gain(pg_ps, simu->gain_ps[idtrat], simu->gain_x, sigma_ps);
interp_gain(pg_focus, simu->gain_focus[idtrat], simu->gain_x, sigma_focus);
}else{
dmat *sigma2=dnew(1,1);
dcell *tmp;
sigma2->p[0]=sigma_tt;
tmp=servo_optim(simu->psd_tt, parms->maos.dt, dtrat, parms->skyc.pmargin, sigma2, servotype);
memcpy(pg_tt, tmp->p[0]->p, (ng+2)*sizeof(double)); dcellfree(tmp);
sigma2->p[0]=sigma_ps;
tmp=servo_optim(simu->psd_ps, parms->maos.dt, dtrat, parms->skyc.pmargin, sigma2, servotype);
memcpy(pg_ps, tmp->p[0]->p, (ng+2)*sizeof(double)); dcellfree(tmp);
if(nmod>5){
sigma2->p[0]=sigma_focus;
tmp=servo_optim(simu->psd_focus, parms->maos.dt, dtrat, parms->skyc.pmargin, sigma2, servotype);
memcpy(pg_focus, tmp->p[0]->p, (ng+2)*sizeof(double)); dcellfree(tmp);
}
dfree(sigma2);
}
res_ngs = pg_tt[ng] + pg_ps[ng] + pg_focus[ng];//residual mode
res_ngsn = pg_tt[ng+1] + pg_ps[ng+1] + pg_focus[ng+1];//error due to noise
for(int imod=0; imod<MIN(nmod,5); imod++){
memcpy(pgain[imod], imod<2?pg_tt:pg_ps, sizeof(double)*ng);
}
if(nmod>5){
memcpy(pgain[5], pg_focus, sizeof(double)*ng);
}
}else{
double pg_ngs[ng+2];
if(parms->skyc.interpg){
interp_gain(pg_ngs, simu->gain_ngs[idtrat], simu->gain_x, sigma_ngs);
}else{
dmat *sigma2=dnew(1,1); sigma2->p[0]=sigma_ngs;
dcell *tmp;
tmp=servo_optim(simu->psd_ngs, parms->maos.dt, dtrat, parms->skyc.pmargin, sigma2, servotype);
memcpy(pg_ngs, tmp->p[0]->p, (ng+2)*sizeof(double)); dcellfree(tmp);
}
res_ngs=pg_ngs[ng];
res_ngsn=pg_ngs[ng+1];
for(int imod=0; imod<nmod; imod++){
memcpy(pgain[imod], pg_ngs, sizeof(double)*ng);
}
}
pres_ngs[0][idtrat]=res_ngs+res_ngsn;/*error due to signal and noise */
pres_ngs[1][idtrat]=res_ngs;/*error due to signal */
pres_ngs[2][idtrat]=res_ngsn;/*error due to noise propagation. */
/*if(parms->skyc.reest){//estiamte error in time domain
dmat *sigma2=dnew(nmod,nmod);PDMAT(sigma2, psigma2);
PDMAT(parms->maos.mcc, pmcc);
//convert noise into mode space from WFE space.
psigma2[0][0]=psigma2[1][1]=sigma_tt/(2*pmcc[0][0]);
psigma2[2][2]=psigma2[3][3]=psigma2[4][4]=sigma_ps/(3*pmcc[2][2]);
if(nmod>5){
psigma2[5][5]=sigma_focus/pmcc[5][5];
}
dmat *res=servo_test(simu->mideal, parms->maos.dt, dtrat, sigma2, aster->gain->p[idtrat]);
double rms=calc_rms(res,parms->maos.mcc, parms->skyc.evlstart);
pres_ngs[0][idtrat]=rms;
dfree(sigma2);
dfree(res);
}*/
dmat *g_tt=dnew_ref(ng,1,pgain[0]);
double gain_n;
aster->res_ws->p[idtrat]=servo_residual(&gain_n, parms->skyc.psd_ws,
parms->maos.dt, dtrat, g_tt, 2);
dfree(g_tt);
}//for dtrat
if(parms->skyc.dbg){
writebin(aster->gain,"%s/aster%d_gain",dirsetup,aster->iaster);
writebin(aster->res_ws,"%s/aster%d_res_ws",dirsetup,aster->iaster);
writebin(aster->res_ngs,"%s/aster%d_res_ngs",dirsetup,aster->iaster);
}
}
/**
Setup the detector transfer functions. See maos/setup_powfs.c
*/
static void setup_powfs_dtf(POWFS_S *powfs, const PARMS_S* parms){
const int npowfs=parms->maos.npowfs;
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const int ncomp=parms->maos.ncomp[ipowfs];
const int ncomp2=ncomp>>1;
const int embfac=parms->maos.embfac[ipowfs];
const int pixpsa=parms->skyc.pixpsa[ipowfs];
const double pixtheta=parms->skyc.pixtheta[ipowfs];
const double blur=parms->skyc.pixblur[ipowfs]*pixtheta;
const double e0=exp(-2*M_PI*M_PI*blur*blur);
const double dxsa=parms->maos.dxsa[ipowfs];
const double pixoffx=parms->skyc.pixoffx[ipowfs];
const double pixoffy=parms->skyc.pixoffy[ipowfs];
const double pxo=-(pixpsa/2-0.5+pixoffx)*pixtheta;
const double pyo=-(pixpsa/2-0.5+pixoffy)*pixtheta;
loc_t *loc_ccd=mksqloc(pixpsa, pixpsa, pixtheta, pixtheta, pxo, pyo);
powfs[ipowfs].dtf=mycalloc(parms->maos.nwvl,DTF_S);
for(int iwvl=0; iwvl<parms->maos.nwvl; iwvl++){
const double wvl=parms->maos.wvl[iwvl];
const double dtheta=wvl/(dxsa*embfac);
const double pdtheta=pixtheta*pixtheta/(dtheta*dtheta);
const double du=1./(dtheta*ncomp);
const double du2=du*du;
const double dupth=du*pixtheta;
cmat *nominal=cnew(ncomp,ncomp);
//cfft2plan(nominal,-1);
//cfft2plan(nominal,1);
cmat* pn=nominal;
const double theta=0;
const double ct=cos(theta);
const double st=sin(theta);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
double ir=ct*jx+st*jy;
double ia=-st*jx+ct*jy;
IND(pn,ix,iy)=sinc(ir*dupth)*sinc(ia*dupth)
*pow(e0,ir*ir*du2)*pow(e0,ia*ia*du2)
*pdtheta;
}
}
if(parms->skyc.fnpsf1[ipowfs]){
warning("powfs %d has additional otf to be multiplied\n", ipowfs);
dcell *psf1c=dcellread("%s", parms->skyc.fnpsf1[ipowfs]);
dmat *psf1=NULL;
if(psf1c->nx == 1){
psf1=dref(psf1c->p[0]);
}else if(psf1c->nx==parms->maos.nwvl){
psf1=dref(psf1c->p[iwvl]);
}else{
error("skyc.fnpsf1 has wrong dimension\n");
}
dcellfree(psf1c);
if(psf1->ny!=2){
error("skyc.fnpsf1 has wrong dimension\n");
}
dmat *psf1x=dnew_ref(psf1->nx, 1, psf1->p);
dmat *psf1y=dnew_ref(psf1->nx, 1, psf1->p+psf1->nx);
dmat *psf2x=dnew(ncomp*ncomp, 1);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
psf2x->p[ix+iy*ncomp]=sqrt(jx*jx+jy*jy)*dtheta;
}
}
info("powfs %d, iwvl=%d, dtheta=%g\n", ipowfs, iwvl, dtheta*206265000);
writebin(psf2x, "powfs%d_psf2x_%d", ipowfs,iwvl);
dmat *psf2=dinterp1(psf1x, psf1y, psf2x, 0);
normalize_sum(psf2->p, psf2->nx*psf2->ny, 1);
psf2->nx=ncomp; psf2->ny=ncomp;
writebin(psf2, "powfs%d_psf2_%d", ipowfs,iwvl);
cmat *otf2=cnew(ncomp, ncomp);
//cfft2plan(otf2, -1);
ccpd(&otf2, psf2);//peak in center
cfftshift(otf2);//peak in corner
cfft2(otf2, -1);
cfftshift(otf2);//peak in center
writebin(otf2, "powfs%d_otf2_%d", ipowfs, iwvl);
writebin(nominal, "powfs%d_dtf%d_nominal_0",ipowfs,iwvl);
for(int i=0; i<ncomp*ncomp; i++){
nominal->p[i]*=otf2->p[i];
}
writebin(nominal, "powfs%d_dtf%d_nominal_1",ipowfs,iwvl);
dfree(psf1x);
dfree(psf1y);
dfree(psf2x);
dfree(psf1);
cfree(otf2);
dfree(psf2);
}
cfftshift(nominal);//peak in corner
cfft2(nominal,-1);
cfftshift(nominal);//peak in center
cfft2i(nominal,1);
warning_once("double check nominal for off centered skyc.fnpsf1\n");
/*This nominal will multiply to OTF with peak in corner. But after
inverse fft, peak will be in center*/
ccp(&powfs[ipowfs].dtf[iwvl].nominal, nominal);
cfree(nominal);
loc_t *loc_psf=mksqloc(ncomp, ncomp, dtheta, dtheta, -ncomp2*dtheta, -ncomp2*dtheta);
powfs[ipowfs].dtf[iwvl].si=mkh(loc_psf,loc_ccd,0,0,1);
locfree(loc_psf);
if(parms->skyc.dbg){
writebin(powfs[ipowfs].dtf[iwvl].nominal,
"%s/powfs%d_dtf%d_nominal",dirsetup,ipowfs,iwvl);
writebin(powfs[ipowfs].dtf[iwvl].si,
"%s/powfs%d_dtf%d_si",dirsetup,ipowfs,iwvl);
}
powfs[ipowfs].dtf[iwvl].U=cnew(ncomp,1);
dcomplex *U=powfs[ipowfs].dtf[iwvl].U->p;
for(int ix=0; ix<ncomp; ix++){
int jx=ix<ncomp2?ix:(ix-ncomp);
U[ix]=COMPLEX(0, -2.*M_PI*jx*du);
}
}/*iwvl */
locfree(loc_ccd);
}/*ipowfs */
}
