/**
Compute Signal level
*/
static void setup_star_siglev(const PARMS_S *parms, STAR_S *star, int nstar){
const long npowfs=parms->maos.npowfs;
const long nwvl=parms->maos.nwvl;
const double r2=pow(parms->skyc.patfov/206265./2.,2);
dmat* rnefs=parms->skyc.rnefs;
for(int istar=0; istar<nstar; istar++){
star[istar].siglev=dcellnew(npowfs, 1);
star[istar].bkgrnd=dnew(npowfs,1);
star[istar].siglevtot=dnew(npowfs,1);
/*Normalized angular distance */
double th2=(pow(star[istar].thetax,2)+pow(star[istar].thetay,2))/r2;
/*Field dependent error: nm^2=nma^2+nmb^2*theta_norm^2; */
double imperrnm=sqrt(pow(parms->skyc.imperrnm,2)+th2*pow(parms->skyc.imperrnmb,2));
for(long ipowfs=0; ipowfs<npowfs; ipowfs++){
star[istar].siglev->p[ipowfs]=dnew(nwvl,1);
int iscircle=parms->maos.nsa[ipowfs]<=4?1:0;
photon_flux(&parms->skyc.zb, star[istar].siglev->p[ipowfs]->p,
&star[istar].siglevtot->p[ipowfs],
&star[istar].bkgrnd->p[ipowfs],
NULL, NULL,
parms->maos.nwvl,
parms->maos.wvl,
star[istar].mags->p,
parms->maos.dxsa[ipowfs], iscircle,
parms->skyc.pixtheta[ipowfs],
parms->maos.dt, parms->maos.za,
NULL,
imperrnm,
parms->skyc.telthruput,
parms->skyc.qe,
IND(rnefs,parms->skyc.ndtrat-1,ipowfs));
if(parms->skyc.verbose && ipowfs==npowfs-1){
info2("star %d at (%5.1f %5.1f)",istar,
star[istar].thetax*206265,star[istar].thetay*206265);
info2(" bkgrnd=%5.2f, pixtheta=%4.1fmas mag=[",
star[istar].bkgrnd->p[ipowfs],parms->skyc.pixtheta[ipowfs]*206265000);
for(int iwvl=0; iwvl<parms->maos.nwvl; iwvl++){
info2("%5.2f ", star[istar].mags->p[iwvl]);
}
info2("] siglev=[");
for(int iwvl=0; iwvl<parms->maos.nwvl; iwvl++){
info2("%6.1f ", star[istar].siglev->p[ipowfs]->p[iwvl]);
}
info2("]\n");
}
}
}
}
void meanMarginalpstategivenx(doubleMatrix result, doubleMatrix trainingData,doubleMatrix A,
doubleMatrix means, doubleMatrix variances, doubleMatrix weights,
double noiseLevel) {
int i, j, k;
double sum;
marginalpstategivenx(pm, trainingData, A, means, variances, weights, noiseLevel);
for (i = 0;i < numberOfGaussians;i++)
for (j = 0;j < dimensions;j++) {
sum = 0;
doubleMatrix *indpm = pm;
for (k = 0;k < numberOfDataRows;k++)
sum += (indpm++)->element[(i*indpm->numberOfRows)+j];
IND(result,i,j) = sum / numberOfDataRows;
}
}
int UpdateVelocities( void* arg, int id ) {
int i;
int finish;
if (id == NTHREADS - 1) {
finish = numMoles;
} else {
finish = (numMoles / NTHREADS) * (id + 1);
}
for (i = (numMoles / NTHREADS) * id; i < finish; ++i) {
f[IND(0,i)] *= timeStepSqHalf ;
f[IND(1,i)] *= timeStepSqHalf ;
f[IND(2,i)] *= timeStepSqHalf ;
vh[ IND(0,i) ] += f[IND(0,i)];
vh[ IND(1,i) ] += f[IND(1,i)];
vh[ IND(2,i) ] += f[IND(2,i)];
}
return 0;
}
//we should use dynamic scheduling here
int BuildNeigh( void* arg, int id )
{
double rd, cutoffSquare;
int i,j;
//double Foo();
cutoffSquare = (cutoffRadius * TOLERANCE)*(cutoffRadius * TOLERANCE);
for (i = id; i < numMoles; i += NTHREADS) {
num_neighbours [i] = 0;
//resetting these two as well
counter_f [i] = 0;
counter_x [i] = 0;
}
//for ( i=0; i<numMoles; i++) //single-threaded version
for ( i=id; i<numMoles; i+=NTHREADS)
{
//BEGIN_TRANSACTION();
//if (id == 0) {
for ( j = i+1; j<numMoles; j++ )
{
//x is global var, would need exclusive access unless this method is surrounded
//by barriers, because while x is only read here (and not written), other methods
//do write to x.
rd = Foo ( (double)x[IND(0,i)], (double)x[IND(1,i)], (double)x[IND(2,i)],
(double)x[IND(0,j)], (double)x[IND(1,j)], (double)x[IND(2,j)]);
if ( rd <= cutoffSquare)
{
//inter[] and ninter are global vars, need exclusive access
//do this more efficiently? atomic read
pthread_mutex_lock (&mutex_ninter);
inter[INDX(ninter,0)] = i;
inter[INDX(ninter,1)] = j;
//make sure atomic_inc works properly, try pthread_mutex_lock to double-check
//atomic_inc (&ninter);
ninter ++;
num_neighbours [i] ++;
num_neighbours [j] ++;
pthread_mutex_unlock (&mutex_ninter);
//this is a useless statement
if ( ninter >= MAXINTERACT) perror("MAXINTERACT limit");
}
}
//COMMIT_TRANSACTION();
//}
}
return 0;
}
/*
* Compute an entire pixmap of palette values.
*/
static void Compute(int xlo, int xhi, double xstart,
int ylo, int yhi, double ystart,
int width, char* map) {
double x;
double y;
int i;
int j;
/* Compute the mandelbrot set, leaving color values in pixmap. */
y = ystart;
for (i = ylo; i < yhi; ++i) {
x = xstart;
for (j = xlo; j < xhi; ++j) {
IND(map, width, i, j) = mandel(x, y);
x += xstep;
}
y += ystep;
}
}
// -------------------------------------------------------------------------
// Privates
// -------------------------------------------------------------------------
static uint32_t pci_read(uint8_t bus, uint8_t dev, uint8_t func, uint8_t reg_offset, uint8_t len)
{
uint32_t ret;
const uint32_t val = 0x80000000 |
(bus << 16) |
(dev << 11) |
(func << 8) |
(reg_offset & 0xFC);
OUTD(PCI_ADDR_IO_PORT, val);
ret = IND(PCI_DATA_IO_PORT + (reg_offset & 0x3));
ret &= (0xFFFFFFFF >> ((4-len) * 8));
return ret;
}
void marginalpstategivenx(doubleMatrix *result, doubleMatrix trainingData,doubleMatrix A,
doubleMatrix means, doubleMatrix variances, doubleMatrix weights,
double noiseLevel) {
int i, j, k, l, *ind, indSize, n;
double sum;
pstategivenx(p, trainingData, A, means, variances, weights, noiseLevel);
n = pow(numberOfGaussians,dimensions);
for (i = 0;i < dimensions;i++)
for (j = 0;j < numberOfGaussians;j++) {
indSize = 0;
for (k = 0;k < n;k++) {
if (states[i][k] - 1 == j) {
indSize++;
}
}
ind = malloc(indSize*sizeof(int));
l = 0;
for (k = 0;k < n;k++)
if (states[i][k] - 1 == j) {
ind[l] = k;
l++;
}
doubleMatrix *indres = result;
for (k = 0;k < numberOfDataRows;k++) {
sum = 0;
for (l = 0;l < indSize;l++)
sum += IND(p,ind[l],k);
(indres++)->element[(j*indres->numberOfRows)+i] = sum;
}
free(ind);
}
}
// write to the pci config space
static void pci_write(uint8_t bus, uint8_t dev, uint8_t func, uint8_t reg_offset, uint8_t len, uint32_t value)
{
uint32_t val = 0x80000000 |
(bus << 16) |
(dev << 11) |
(func << 8) |
(reg_offset & 0xFC);
OUTD(PCI_ADDR_IO_PORT, val);
// get current value
val = IND(PCI_DATA_IO_PORT + (reg_offset & 0x3));
// mask out new section
if (len != 4)
val &= (0xFFFFFFFF << (len * 8));
else
val = 0; // vc++/processor does not allow shift counts of 32
val |= value;
OUTD(PCI_DATA_IO_PORT + (reg_offset & 0x3), val);
}
/*---------------------------------------------------------------------------
* sio_set_index_data_reg
*
* This routine checks which index and data registers to use
* in order to access the Super I/O registers
*
* Returns : 1 - OK
* 0 - Super I/O disabled or configuration access disabled
*
*---------------------------------------------------------------------------
*/
int
sio_set_index_data_reg(void)
{
unsigned long xbus_expention_bar, io_control_reg1;
OsPciReadDWord(0, 0x12, 5, 0x10, &xbus_expention_bar);
xbus_expention_bar = xbus_expention_bar & 0xfffffffe;
io_control_reg1 = IND((unsigned short) xbus_expention_bar);
if ((io_control_reg1) & (SIO_BASE_ADR_15C_15D)) {
index_reg = INDEX_1;
data_reg = DATA_1;
return (1);
}
if ((io_control_reg1) & (SIO_BASE_ADR_2E_2F)) {
index_reg = INDEX_2;
data_reg = DATA_2;
return (1);
}
return (0);
}
int ComputeKE( void* arg, int id ){
double sum = 0.0;
int i;
int finish;
if (id == NTHREADS - 1) finish = numMoles;
else finish = (numMoles / NTHREADS) * (id + 1);
for ( i = (numMoles / NTHREADS ) * id; i < finish; i++) {
sum = sum + vh[ IND(0,i) ] * vh[ IND(0,i) ];
sum = sum + vh[ IND(1,i) ] * vh[ IND(1,i) ];
sum = sum + vh[ IND(2,i) ] * vh[ IND(2,i) ];
}
BEGIN_TRANSACTION();
ekin += sum/timeStepSq ;
COMMIT_TRANSACTION();
return 0;
}
/* evaluoi IF-mallin (kts. evaluateObj.m) */
double evaluateObj(doubleMatrix trainingData,doubleMatrix A, doubleMatrix means,
doubleMatrix variances, doubleMatrix weights, double noiseLevel) {
int i, j, k;
int *q;
double pq;
doubleMatrix f = createDoubleMatrix(1,numberOfDataRows);
for (i = 0;i < numberOfDataRows;i++)
IND(f,0,i) = 0;
double obj = 0;
for (i = 0;i < lambda.numberOfRows;i++)
for (j = 0;j < lambda.numberOfColumns;j++) {
if (i == j)
IND(lambda,i,j) = noiseLevel;
else
IND(lambda,i,j) = 0;
}
for (i = 0;i < diagvq.numberOfRows;i++)
for (j = 0;j < diagvq.numberOfColumns;j++) {
IND(diagvq,i,j) = 0;
}
for (i = 0;i < numberOfStates;i++) {
double a;
q = getState(i);
pq = 1;
for (j = 0;j < dimensions;j++) {
pq *= IND(weights,q[j],j);
IND(mq,j,0) = IND(means,q[j],j);
IND(vq,j,0) = IND(variances,q[j],j);
}
multiply(A, mq, meanq);
for (j = 0;j < dimensions;j++)
IND(diagvq,j,j) = IND(vq,j,0);
multiply(A, diagvq, Adiagvq);
multiplyABt(Adiagvq, A, AdiagvqAt);
add(AdiagvqAt, lambda, sigmaq);
double *indXm = Xm.element;
double *indmq = meanq.element;
double *indTD = trainingData.element;
for (j = 0;j < Xm.numberOfColumns;j++) {
for (k = 0;k < Xm.numberOfRows;k++)
*(indXm++) = *(indTD++) - *(indmq++);
indmq = meanq.element;
}
multiplyConst(sigmaq,2*M_PI, csigmaq);
a = 1.0 / sqrt(fabs(determinant(csigmaq)));
inverse(sigmaq, invsigmaq);
multiply(invsigmaq, Xm, invsigmaqXm);
indXm = Xm.element;
double *indinv = invsigmaqXm.element;
for (k = 0;k < numberOfDataRows;k++) {
double sum = 0;
for (j = 0;j < numberOfObserved;j++)
sum += (*(indXm++))*(*(indinv++));
IND(f,0,k) += pq *(a *exp(-0.5*sum));
}
free(q);
}
for (i = 0;i < numberOfDataRows;i++) {
if (IND(f,0,i) < 0) {
freeDoubleMatrix(f);
return FLT_MAX;
}
else
obj += log(IND(f,0,i));
}
freeDoubleMatrix(f);
return (-obj / numberOfDataRows);
}
/* skaalaa mallin siten, että lähteillä yksikkövarianssi ja nollakeskisyys */
void rescaleModel(doubleMatrix A,doubleMatrix means,doubleMatrix variances,doubleMatrix weights) {
int i, j;
double* sigmaSq = malloc(dimensions*sizeof(double));
for (i = 0;i < dimensions;i++) {
double sum1 = 0;
double sum2 = 0;
for (j = 0;j < numberOfGaussians;j++) {
sum1 += IND(weights,j,i)*(IND(variances,j,i) +
pow(IND(means,j,i),2));
sum2 += IND(weights,j,i)*IND(means,j,i);
}
sigmaSq[i] = sum1 - pow(sum2,2);
}
for (i = 0;i < numberOfGaussians;i++)
for (j = 0;j < dimensions;j++) {
IND(means,i,j) = IND(means,i,j) / sqrt(sigmaSq[j]);
IND(variances,i,j) = IND(variances,i,j) / sigmaSq[j];
}
for (i = 0;i < numberOfObserved;i++)
for (j = 0;j < dimensions;j++)
IND(A,i,j) = IND(A,i,j) * sqrt(sigmaSq[j]);
free(sigmaSq);
}
/* pääfunktio ****************************/
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
doubleMatrix trainingData, A, means, variances, weights;
double noiseLevel;
int numberOfIterations, updateDistributions;
int n, k, i, j, l;
/* parametrit */
trainingData = createDoubleMatrixMx(TRAININGDATA_IN);
A = createDoubleMatrixMx(A_IN);
means = createDoubleMatrixMx(MEANS_IN);
variances = createDoubleMatrixMx(VARIANCES_IN);
weights = createDoubleMatrixMx(WEIGHTS_IN);
noiseLevel = mxGetScalar(NOISELEVEL_IN);
numberOfIterations = (int)mxGetScalar(MAX_ITERATIONS_IN);
updateDistributions = (int)mxGetScalar(UPDATE_DISTRIBUTIONS);
/* globaaleiden muuttujien alustus */
numberOfGaussians = mxGetM(MEANS_IN);
dimensions = mxGetN(MEANS_IN);
numberOfObserved = mxGetM(TRAININGDATA_IN);
numberOfDataRows = mxGetN(TRAININGDATA_IN);
numberOfLatent = dimensions - numberOfObserved;
states = statelist();
numberOfStates = pow(numberOfGaussians,dimensions);
/* skaalaus */
doubleMatrix dummyA = createDoubleMatrix(numberOfObserved,dimensions);
rescaleModel(dummyA,means,variances,weights);
freeDoubleMatrix(dummyA);
/* virhefunktion arvot */
doubleMatrix objMatrix = createDoubleMatrix(2,numberOfIterations / RANGE);
l = 0;
n = dimensions*dimensions;
/* alustetaan globaalit matriisit */
matrixInitialize();
/* EM-algoritmin pääsilmukka */
for (k = 0;k < numberOfIterations;k++) {
/* painojen päivitys */
if (updateDistributions)
meanMarginalpstategivenx(weights, trainingData, A, means, variances, weights, noiseLevel);
/* A:n päivitys */
meanssgivenx(mgssx, mgsx, trainingData, A, means, variances, weights, noiseLevel);
multiplyABt(trainingData, mgsx, XmgsxTran);
multiplyConst(XmgsxTran,1.0 / numberOfDataRows, XmgsxTranDiv);
for (i = 0;i < n;i++)
IND(meanMgssx,i,0) = 0;
for (i = 0;i < n;i++)
for (j = 0;j < numberOfDataRows;j++)
IND(meanMgssx,i,0) += IND(mgssx,i,j) / numberOfDataRows;
for (i = 0;i < dimensions;i++)
for (j = 0;j < dimensions;j++)
IND(meanMgssx2,j,i) = IND(meanMgssx,i*dimensions+j,0);
inverse(meanMgssx2, invMeanMgssx);
multiply(XmgsxTranDiv, invMeanMgssx, A);
/* varianssien ja keskiarvojen päivitys */
if (updateDistributions) {
meanMarginalpstategivenx(mmpgx, trainingData, A, means, variances, weights, noiseLevel);
a11meanx2(ax2, ax, trainingData, A, means, variances, weights, noiseLevel);
for (i = 0;i < numberOfGaussians;i++)
for (j = 0;j < dimensions;j++)
IND(means,i,j) = IND(ax,i,j) / IND(mmpgx,i,j);
for (i = 0;i < numberOfGaussians;i++)
for (j = 0;j < dimensions;j++)
IND(variances,i,j) = IND(ax2,i,j) / IND(mmpgx,i,j)
- pow(IND(means,i,j),2);
}
/* skaalataan IF-malli */
rescaleModel(A,means,variances,weights);
if (k % RANGE == 0) {
/* tarkistetaan A:n validisuus (ei sisällä pelkästään nollia) */
/* if (!isValid(variances)) { */
if (IND(A,0,0) - NAN < EPSILON) {
/* jos ei enää validi, niin laitetaan tästä eteenpäin virhefunktiolle
vain Inf -arvoa */
while (l < numberOfIterations / RANGE) {
IND(objMatrix,0,l) = FLT_MAX;
IND(objMatrix,1,l++) = k;
}
/* tulostetaan varianssit ja häivytään silmukasta */
printDoubleMatrix(variances);
break;
} else {
IND(objMatrix,0,l) = evaluateObj(trainingData, A, means, variances, weights, noiseLevel);
IND(objMatrix,1,l++) = k;
}
}
}
/* vapautetaan globaaleiden matriisien varaama muisti*/
matrixFinalize();
/* ulostulot */
A_OUT = createMxArray(A);
MEANS_OUT = createMxArray(means);
VARIANCES_OUT = createMxArray(variances);
WEIGHTS_OUT = createMxArray(weights);
OBJ_OUT = createMxArray(objMatrix);
free2d(states,dimensions);
freeDoubleMatrix(trainingData);
freeDoubleMatrix(A);
freeDoubleMatrix(means);
freeDoubleMatrix(variances);
freeDoubleMatrix(weights);
freeDoubleMatrix(objMatrix);
return;
}
/* Evaluates the determinant of matrix A */
double determinant(doubleMatrix A) {
int i,j,k,u;
double temp, a, det;
doubleMatrix B = BdetInv;
for(i = 0;i < A.numberOfColumns;i++)
for(j = 0;j < A.numberOfRows;j++)
IND(B,i,j) = IND(A,i,j);
j = 0;
for (i = 0;i < B.numberOfRows;i++) {
if (fabs(IND(B,i,j)) < EPSILON) {
for (k = i+1;k < B.numberOfRows;k++)
if (!(fabs(IND(B,k,j)) < EPSILON)) {
for (u = 0;u < B.numberOfColumns;u++) {
temp = IND(B,i,u);
IND(B,i,u) = IND(B,k,u);
IND(B,k,u) = temp;
}
}
if (fabs(IND(B,i,j)) < EPSILON)
return 0;
}
for (k = i+1;k < B.numberOfRows;k++) {
/* subtract A[u,j] * row i from row u */
a = IND(B,k,j) / IND(B,i,j);
for (u = 0;u < B.numberOfColumns;u++) {
IND(B,k,u) = IND(B,k,u) - a*IND(B,i,u);
}
}
j++;
}
det = IND(B,0,0);
for(i = 1;i < A.numberOfColumns;i++)
det *= IND(B,i,i);
return det;
}
/* Laskee matriisin A käänteismatriisin. Luotu pseudokoodin pohjalta,
joka löytyy osoitteesta
http://en.wikipedia.org/wiki/Gauss-Jordan_elimination
*/
void inverse(doubleMatrix A, doubleMatrix C) {
int i,j,k,u,v,max_ind;
double val, max_val, temp, temp2, a;
doubleMatrix B;
if (A.numberOfRows == numberOfObserved)
B = BdetInv;
else
B = Binv2;
for(i = 0;i < A.numberOfColumns;i++) {
for(j = 0;j < A.numberOfRows;j++) {
IND(B,i,j) = IND(A,i,j);
if (i == j)
IND(C,i,j) = 1.0;
else
IND(C,i,j) = 0.0;
}
}
i = j = k = u = v = 0;
while (i < A.numberOfRows && j < A.numberOfColumns) {
/* Find pivot in column j, starting in row i */
/* max_val = B_(i,j); */
max_val = IND(B,i,j);
max_ind = i;
for (k = i + 1;k < A.numberOfRows;k++) {
/* val = B_(k,j); */
val = IND(B,k,j);
if (abs(val) > abs(max_val)) {
max_val = val;
max_ind = k;
}
}
if (fabs(max_val) > EPSILON) {
/* switch rows i and max_ind */
for (u = 0;u < A.numberOfColumns;u++) {
/* temp = B_(i,u); */
temp = IND(B,i,u);
temp2 = IND(C,i,u);
/* B_(i,u) = B_(max_ind,u); */
IND(B,i,u) = IND(B,max_ind,u);
IND(C,i,u) = IND(C,max_ind,u);
/* B_(max_ind,u) = temp; */
IND(B,max_ind,u) = temp;
IND(C,max_ind,u) = temp2;
}
/* divide row i by max_val */
for (u = 0;u < A.numberOfColumns;u++) {
/* B_(i,u) = B_(i,u) / max_val; */
IND(B,i,u) = IND(B,i,u) / max_val;
IND(C,i,u) = IND(C,i,u) / max_val;
}
for (u = 0;u < A.numberOfColumns;u++) {
if (u != i) {
/* subtract A[u,j] * row i from row u */
a = IND(B,u,j);
for (v = 0;v < A.numberOfColumns;v++) {
IND(B,u,v) = IND(B,u,v) - a*IND(B,i,v);
IND(C,u,v) = IND(C,u,v) - a*IND(C,i,v);
}
}
}
i++;
}
j++;
}
}
static void test_psd(){
rand_t rstat;
int seed=4;
double r0=0.2;
double dx=1./64;
long N=1024;
long nx=N;
long ny=N;
long ratio=1;
long xskip=nx*(ratio-1)/2;
long yskip=ny*(ratio-1)/2;
long nframe=512;
seed_rand(&rstat, seed);
if(1){
map_t *atm=mapnew(nx+1, ny+1, dx,dx, NULL);
cmat *hat=cnew(nx*ratio, ny*ratio);
//cfft2plan(hat, -1);
dmat *hattot=dnew(nx*ratio, ny*ratio);
for(long i=0; i<nframe; i++){
info2("%ld of %ld\n", i, nframe);
for(long j=0; j<(nx+1)*(ny+1); j++){
atm->p[j]=randn(&rstat);
}
fractal_do((dmat*)atm, dx, r0,L0,ninit);
czero(hat);
for(long iy=0; iy<ny; iy++){
for(long ix=0; ix<nx; ix++){
IND(hat,ix+xskip,iy+yskip)=IND(atm,ix,iy);
}
}
cfftshift(hat);
cfft2i(hat, -1);
cabs22d(&hattot, 1, hat, 1);
}
dscale(hattot, 1./nframe);
dfftshift(hattot);
writebin(hattot, "PSD_fractal");
}
{
dmat *spect=turbpsd(nx, ny, dx, r0, 100, 0, 0.5);
writebin(spect, "spect");
cmat *hat=cnew(nx*ratio, ny*ratio);
//cfft2plan(hat, -1);
dmat *hattot=dnew(nx*ratio, ny*ratio);
cmat *atm=cnew(nx, ny);
//cfft2plan(atm, -1);
dmat *atmr=dnew(atm->nx, atm->ny);
dmat *atmi=dnew(atm->nx, atm->ny);
cmat* phat=hat;
dmat* patmr=atmr;
dmat* patmi=atmi;
for(long ii=0; ii<nframe; ii+=2){
info2("%ld of %ld\n", ii, nframe);
for(long i=0; i<atm->nx*atm->ny; i++){
atm->p[i]=COMPLEX(randn(&rstat), randn(&rstat))*spect->p[i];
}
cfft2(atm, -1);
for(long i=0; i<atm->nx*atm->ny; i++){
atmr->p[i]=creal(atm->p[i]);
atmi->p[i]=cimag(atm->p[i]);
}
czero(hat);
for(long iy=0; iy<ny; iy++){
for(long ix=0; ix<nx; ix++){
IND(phat,ix+xskip,iy+yskip)=IND(patmr,ix,iy);
}
}
cfftshift(hat);
cfft2i(hat, -1);
cabs22d(&hattot, 1, hat, 1);
czero(hat);
for(long iy=0; iy<ny; iy++){
for(long ix=0; ix<nx; ix++){
IND(phat,ix+xskip,iy+yskip)=IND(patmi,ix,iy);
}
}
cfftshift(hat);
cfft2i(hat, -1);
cabs22d(&hattot, 1, hat, 1);
}
dscale(hattot, 1./nframe);
dfftshift(hattot);
writebin(hattot, "PSD_fft");
}
}
/*IND*/
static void op_ED_0xaa(Z80EX_CONTEXT *cpu)
{
IND(/*rd*/6, /*wr*/9);
T_WAIT_UNTIL(12);
return;
}
void a11meanx2(doubleMatrix result, doubleMatrix result2, doubleMatrix trainingData,
doubleMatrix A, doubleMatrix means, doubleMatrix variances,
doubleMatrix weights, double noiseLevel) {
int *q, i, j, k, l, ind, n;
pstategivenx(p, trainingData, A, means, variances, weights, noiseLevel);
n = dimensions*numberOfGaussians;
doubleMatrix *indr = r;
doubleMatrix *indr2 = r2;
double *indrel = (*indr).element;
double *indrel2 = (*indr2).element;
for (k = 0;k < numberOfDataRows;k++) {
for (i = 0;i < n;i++) {
*(indrel++) = 0;
*(indrel2++) = 0;
}
indrel = (*(++indr)).element;
indrel2 = (*(++indr2)).element;
}
for (i = 0;i < dimensions;i++)
for (j = 0;j < numberOfGaussians;j++)
for (k = 0;k < numberOfStates;k++) {
indr = r;
indr2 = r2;
q = getState(k);
if (q[i] == j) {
meanssgivenqx(ss, s, q, trainingData, A, means, variances, weights, noiseLevel);
double *ssind = &IND(ss,i*dimensions + i,0);
double *sind = &IND(s,i,0);
double *pind = &IND(p,k,0);
for (l = 0;l < numberOfDataRows;l++) {
(indr++)->element[(j*indr->numberOfRows)+i] +=
(*(pind + l*p.numberOfRows))*(*(ssind +
l*ss.numberOfRows));
(indr2++)->element[(j*indr2->numberOfRows)+i] +=
(*(pind + l*p.numberOfRows))*(*(sind + l*s.numberOfRows));
}
}
free(q);
}
double *indres = result.element;
double *indres2 = result2.element;
for (i = 0;i < dimensions;i++)
for (j = 0;j < numberOfGaussians;j++) {
double sum = 0;
double sum2 = 0;
indr = r;
indr2 = r2;
for (k = 0;k < numberOfDataRows;k++) {
sum += (indr++)->element[(j*indr->numberOfRows)+i];
sum2 += (indr2++)->element[(j*indr2->numberOfRows)+i];
}
*(indres++) = sum / numberOfDataRows;
*(indres2++) = sum2 / numberOfDataRows;
}
}
int UpdateCoordinates( void* arg, int id ) {
for (int i = (numMoles / NTHREADS ) * id; i < (numMoles / NTHREADS) * (id + 1); ++i) {
vh[IND(0,i)] += f[IND(0,i)];
x[IND(0,i)] += vh[IND(0,i)];
if ( x[IND(0,i)] < 0.0 ) x[IND(0,i)] += side ;
if ( x[IND(0,i)] > side ) x[IND(0,i)] -= side ;
f[IND(0,i)] = 0.0;
vh[IND(1,i)] += f[IND(1,i)];
x[IND(1,i)] += vh[IND(1,i)];
if ( x[IND(1,i)] < 0.0 ) x[IND(1,i)] += side ;
if ( x[IND(1,i)] > side ) x[IND(1,i)] -= side ;
f[IND(1,i)] = 0.0;
vh[IND(2,i)] += f[IND(2,i)];
x[IND(2,i)] += vh[IND(2,i)];
if ( x[IND(2,i)] < 0.0 ) x[IND(2,i)] += side ;
if ( x[IND(2,i)] > side ) x[IND(2,i)] -= side ;
f[IND(2,i)] = 0.0;
}
return 0;
}
int main(int argc, char *argv[])
{
double ttotal = -wtime();
int rows = (argc> 1) ? atoi(argv[1]) : 100;
int cols = (argc> 2) ? atoi(argv[2]) : 100;
const char *filename = (argc> 3) ? argv[3] : NULL;
if (cols < 1 || rows < 1) {
fprintf(stderr, "Invalid size of grid: rows %d, cols %d\n", rows, cols);
exit(EXIT_FAILURE);
}
int ny = rows;
int nx = cols;
double talloc = -wtime();
double *local_grid = malloc(ny * nx * sizeof(*local_grid));
double *local_newgrid = malloc(ny * nx * sizeof(*local_newgrid));
talloc += wtime();
double tinit = -wtime();
double dx = 1.0 / (nx - 1.0);
for (int j = 0; j < nx; j++) {
int ind = IND(0, j);
local_newgrid[ind] = local_grid[ind] = sin(PI * dx * j);
}
for (int j = 0; j < nx; j++) {
int ind = IND(ny -1, j);
local_newgrid[ind] = local_grid[ind] = sin(PI * dx * j) * exp(-PI);
}
for (int i = 1; i < ny - 1; i++) {
for (int j = 1; j < nx - 1; j++) {
local_newgrid[IND(i, j)] = 0.0;
local_grid[IND(i, j)] = 0.0;
}
}
tinit += wtime();
int niters = 0;
for (; ;) {
niters++;
for (int i = 1; i < ny - 1; i++) {
for (int j = 1; j < nx - 1; j++) {
local_newgrid[IND(i, j)] = (local_grid[IND(i - 1, j)] + local_grid[IND(i + 1, j)] + local_grid[IND(i, j - 1)] + local_grid[IND(i, j + 1)]) * 0.25;
}
}
double maxdiff = -DBL_MAX;
for (int i = 1; i < ny - 1; i++) {
for (int j = 1; j < nx - 1; j++) {
int ind = IND(i, j);
maxdiff = fmax(maxdiff, fabs(local_grid[ind] - local_newgrid[ind]));
}
}
double *p = local_grid;
local_grid = local_newgrid;
local_newgrid = p;
if (maxdiff < EPS) break;
}
ttotal += wtime();
printf("# Heat 2D (serial): grid: rows %d, cols %d\n", rows, cols);
printf("# niters %d, total time (sec.): %.6f\n", niters, ttotal);
printf("# talloc: %.6f, tinit: %.6f, titers: %.6f\n", talloc, tinit, ttotal - talloc - tinit);
if (filename) {
FILE *fout = fopen(filename, "w");
if (!fout) {
perror("Can't open file");
exit(EXIT_FAILURE);
}
for (int i = 0; i < ny; i++) {
for (int j = 0; j < nx; j++)
fprintf(fout, "%.4f ", local_grid[IND(i, j)]);
fprintf(fout, "\n");
}
fclose(fout);
}
return 0;
}
/**
Find vibration peaks in the PSD by comparing the PSD against a LPF version plus noise.
*/
dmat *psd_vibid(const dmat *psdin){
double *f=psdin->p;
double *psd=psdin->p+psdin->nx;
dmat *y=dsub(psdin, 0, 0, 1, 1);
const double gain=0.1;
const double gain2=0.05;
int inpeak=0;
double ylpf0=y->p[1];
double dylpf0=fabs(y->p[1]-y->p[0]);
double ylpf=0, dylpf=0;
int nmaxp=100;
dmat *res=dnew(4, nmaxp);
double thres=25e-18;/*threshold: 5 nm*/
double sumxy=0, sumy=0, sum=0;
int count=0;
for(long i=1; i<psdin->nx-1; i++){
if(!inpeak){
//second order LPF
ylpf0=(1.-gain)*ylpf0+y->p[i]*gain;
ylpf=(1.-gain)*ylpf+ylpf0*gain;
double diff=y->p[i]-y->p[i-1];
if(diff>0){
dylpf0=(1.-gain2)*dylpf0+diff*gain2;
dylpf=(1.-gain2)*dylpf+dylpf0*gain2;
}
if(y->p[i+1]>ylpf+dylpf*5 && f[i]>1){//beginning of peak
inpeak=1;
if(count>0 && f[i] < f[(int)IND(res,3,count-1)] + 0.1){
//combine with last peak if within 1 Hz.
count--;
}else{
IND(res,2,count)=i;
sumxy=f[i]*psd[i];//for CoG
sumy=psd[i];//for CoG
sum=0;//integration
}
}
}else{
//continuation of peak
sumxy+=f[i]*psd[i];
sumy+=psd[i];
sum+=(f[i]-f[i-1])*(psd[i]+psd[i-1]);
if(y->p[i]<ylpf+dylpf && y->p[i+1]<ylpf+dylpf){//end of peak
inpeak=0;
if(sum*0.5>thres){
IND(res,0,count)=sumxy/sumy;
IND(res,1,count)=sum*0.5;
IND(res,3,count)=i;
count++;
if(count==nmaxp){
nmaxp*=2;
dresize(res, 4, nmaxp);
}
}
}
}
}
dfree(y);
dresize(res, 4, count);
return res;
}
void pstategivenx(doubleMatrix result, doubleMatrix trainingData,doubleMatrix A,
doubleMatrix means, doubleMatrix variances, doubleMatrix weights,
double noiseLevel) {
int i, j, k;
int *q;
double pq;
for (i = 0;i < lambda.numberOfRows;i++)
for (j = 0;j < lambda.numberOfColumns;j++) {
if (i == j)
IND(lambda,i,j) = noiseLevel;
else
IND(lambda,i,j) = 0;
}
for (i = 0;i < diagvq.numberOfRows;i++)
for (j = 0;j < diagvq.numberOfColumns;j++) {
IND(diagvq,i,j) = 0;
}
for (i = 0;i < numberOfStates;i++) {
double a;
q = getState(i);
pq = 0;
for (j = 0;j < dimensions;j++) {
pq += log(IND(weights,q[j],j));
IND(mq,j,0) = IND(means,q[j],j);
IND(vq,j,0) = IND(variances,q[j],j);
}
multiply(A, mq, meanq);
for (j = 0;j < dimensions;j++)
IND(diagvq,j,j) = IND(vq,j,0);
multiply(A, diagvq, Adiagvq);
multiplyABt(Adiagvq, A, AdiagvqAt);
add(AdiagvqAt, lambda, sigmaq);
double *indXm = Xm.element;
double *indmq = meanq.element;
double *indTD = trainingData.element;
for (j = 0;j < Xm.numberOfColumns;j++) {
for (k = 0;k < Xm.numberOfRows;k++)
*(indXm++) = *(indTD++) - *(indmq++);
indmq = meanq.element;
}
multiplyConst(sigmaq,2*M_PI, csigmaq);
a = log(1.0 / sqrt(fabs(determinant(csigmaq))));
inverse(sigmaq, invsigmaq);
multiply(invsigmaq, Xm, invsigmaqXm);
indXm = Xm.element;
double *indinv = invsigmaqXm.element;
for (k = 0;k < numberOfDataRows;k++) {
double sum = 0;
for (j = 0;j < numberOfObserved;j++)
sum += (*(indXm++))*(*(indinv++));
IND(result,i,k) = pq + a - 0.5*sum;
}
free(q);
}
double *indres;
for (j = 0;j < result.numberOfColumns;j++) {
indres = &IND(result,0,j);
double max = *(indres++);
for (i = 1;i < result.numberOfRows;i++)
if (*(indres++) > max)
max = *(indres - 1);
indres = &IND(result,0,j);
double sum = 0;
for (i = 0;i < result.numberOfRows;i++) {
*indres = exp(*indres-max);
sum += *(indres++);
}
indres = &IND(result,0,j);
for (i = 0;i < result.numberOfRows;i++)
*(indres++) = *indres / sum;
}
}
static void BuildMask(int width, char* map, char* mask) {
int i;
int j;
unsigned int total;
if (x_left == -MAXFLOAT &&
y_top == MAXFLOAT &&
x_right == MAXFLOAT &&
y_bottom == -MAXFLOAT) {
return;
}
/* Compute a nine-point stencil around the current point */
for (i = 1; i < width - 1; ++i) {
for (j = 1; j < width - 1; ++j) {
total = (IND(map, width, i - 1, j - 1) + IND(map, width, i - 1, j) +
IND(map, width, i - 1, j + 1) +
IND(map, width, i, j - 1) + IND(map, width, i, j + 1) +
IND(map, width, i + 1, j - 1) + IND(map, width, i + 1, j) +
IND(map, width, i + 1, j + 1)) >> 3;
if (total == IND(map, width, i, j)) {
IND(mask, width, i, j) = 1;
} else {
IND(mask, width, i, j) = 0;
}
}
}
}
int ComputeForces( void* arg, int id ) {
double cutoffSquare;
double xx, yy, zz, rd, rrd, rrd2, rrd3, rrd4, rrd6, rrd7, r148; // rrd5 never used, removed
double forcex, forcey, forcez;
int i, k;
double vir_tmp, epot_tmp;
cutoffSquare = cutoffRadius*cutoffRadius ;
vir_tmp = 0.0;
epot_tmp = 0.0;
int finish_outer;
if (id == NTHREADS - 1) {
finish_outer = ninter;
} else {
finish_outer = (ninter / NTHREADS) * (id + 1);
}
for(int ii = (ninter / NTHREADS) * id; ii < finish_outer; ++ii) {
i = inter[INDX(ii,0)];
k = inter[INDX(ii,1)];
xx = x[IND(0,i)] - x[IND(0,k)];
yy = x[IND(1,i)] - x[IND(1,k)];
zz = x[IND(2,i)] - x[IND(2,k)];
if (xx < -sideHalf) xx += side;
if (yy < -sideHalf) yy += side;
if (zz < -sideHalf) zz += side;
if (xx > sideHalf) xx -= side;
if (yy > sideHalf) yy -= side;
if (zz > sideHalf) zz -= side;
rd = (xx*xx + yy*yy + zz*zz);
if ( rd < cutoffSquare ) {
rrd = 1.0/rd;
rrd2 = rrd*rrd ;
rrd3 = rrd2*rrd ;
rrd4 = rrd2*rrd2 ;
rrd6 = rrd2*rrd4;
rrd7 = rrd6*rrd ;
r148 = rrd7 - 0.5 * rrd4 ;
forcex = xx*r148;
forcey = yy*r148;
forcez = zz*r148;
BEGIN_TRANSACTION();
f[IND(0,i)] += forcex ;
f[IND(1,i)] += forcey ;
f[IND(2,i)] += forcez ;
f[IND(0,k)] -= forcex ;
f[IND(1,k)] -= forcey ;
f[IND(2,k)] -= forcez ;
COMMIT_TRANSACTION();
vir_tmp += rd*r148 ;
epot_tmp += (rrd6 - rrd3);
}
}
BEGIN_TRANSACTION();
vir -= vir_tmp ;
epot += epot_tmp;
COMMIT_TRANSACTION();
return 0;
}
void meanssgivenqx(doubleMatrix result, doubleMatrix result2, int *q, doubleMatrix trainingData,
doubleMatrix A, doubleMatrix means, doubleMatrix variances, doubleMatrix weights,
double noiseLevel) {
int k, n;
int i, j;
for (j = 0;j < dimensions;j++) {
IND(mq,j,0) = IND(means,q[j],j);
IND(vq,j,0) = IND(variances,q[j],j);
}
for (i = 0;i < iLambda.numberOfRows;i++)
for (j = 0;j < iLambda.numberOfColumns;j++) {
if (i == j)
IND(iLambda,i,j) = 1 / noiseLevel;
else
IND(iLambda,i,j) = 0;
}
for (i = 0;i < vqInv.numberOfRows;i++)
for (j = 0;j < vqInv.numberOfColumns;j++) {
if (i == j)
IND(vqInv,i,j) = 1 / IND(vq,j,0);
else
IND(vqInv,i,j) = 0;
}
multiplyAtB(A, iLambda, AtiLambda);
multiply(AtiLambda, A, AtiLambdaA);
add(AtiLambdaA, vqInv, invsigmaq2);
inverse(invsigmaq2, sigmaq2);
multiply(AtiLambda, trainingData, AtiLambdaX);
multiply(vqInv, mq, vqinvMq);
double *inds2 = s2.element;
double *indvqinvMq;
double *indAtiLambdaX = AtiLambdaX.element;
for (j = 0;j < numberOfDataRows;j++) {
indvqinvMq = vqinvMq.element;
for (i = 0;i < dimensions;i++)
*(inds2++) = *(indvqinvMq++) + *(indAtiLambdaX++);
}
multiply(sigmaq2, s2, result2);
doubleMatrix *indssA = ssArray;
double* indssAel = indssA->element;
double* indsigmaq2;
for (i = 0;i < numberOfDataRows;i++) {
indsigmaq2 = sigmaq2.element;
for (j = 0;j < dimensions;j++)
for (k = 0;k < dimensions;k++) {
*indssAel = IND(result2,j,i)*IND(result2,k,i);
*(indssAel++) += *(indsigmaq2++);
}
indssAel = (++indssA)->element;
}
double *indres = result.element;
indssA = ssArray;
indssAel = indssA->element;
for (k = 0;k < numberOfDataRows;k++) {
for (i = 0;i < dimensions;i++)
for (j = 0;j < dimensions;j++)
*(indres++) = *(indssAel++);
indssAel = (++indssA)->element;
}
}
/**
Compute Von Karman covariance function at separations computed from the grid
size nx and sampling dx, with Fried parameter of r0, and outerscale of L0.
ninit is the initial side size of the atm array to start with.
*/
static vkcov_t* vkcov_calc(double r0, double L0, double dx, long n, long ninit){
if(L0>9000) L0=INFINITY;/*L0 bigger than 9000 is treated as infinity. */
vkcov_t *node=vkcov_get(r0, L0, dx, n, ninit);
if(node) return node;
node=mycalloc(1,vkcov_t);
node->r0=r0;
node->L0=L0;
node->dx=dx;
node->n=n;
node->ninit=ninit;
long nroot=(long)round(log2((double)n-1));
node->next=head;
if(r0>=L0){
error("Illegal parameter: r0=%g, L0=%g\n", r0, L0);
}
head=node;
dmat *r=dnew(2, nroot+2);
double sqrt2=sqrt(2);
IND(r,0,0)=0;
IND(r,1,0)=0;
for(long i=0; i<=nroot; i++){
long j=1<<i;
IND(r,0,i+1)=j*dx;
IND(r,1,i+1)=j*dx*sqrt2;
}
double D=(n-1)*dx;
node->cov=turbcov(r, D*sqrt(2), r0, L0);
dfree(r);
dmat *rc0=dnew(ninit*ninit, ninit*ninit);
dmat* rc=rc0;
double dx2=dx*(n-1)/(ninit-1);
for(long j=0; j<ninit; j++){
double y=dx2*j;
for(long i=0; i<ninit; i++){
double x=dx2*i;
long k=i+j*ninit;
for(long j2=0; j2<ninit; j2++){
double y2=dx2*j2;
for(long i2=0; i2<ninit; i2++){
double x2=dx2*i2;
long k2=i2+j2*ninit;
IND(rc,k2,k)=sqrt((x-x2)*(x-x2)+(y-y2)*(y-y2));
}
}
}
}
node->C=turbcov(rc0, D*sqrt(2), r0, L0);
dfree(rc0);
dmat *u=NULL, *s=NULL, *v=NULL;
dsvd(&u, &s, &v, node->C);
dcwpow(s, 1./2.);
node->K=ddup(u);
dmuldiag(node->K, s);
dcwpow(s, -1);
dmuldiag(u, s);
node->KI=dtrans(u);
dfree(u);
dfree(v);
dfree(s);
/*we have: K*K'==C */
return node;
}
/**
Setup the least square reconstruct by directly inverting GA matrix.
The reconstructor is simply the pseudo inverse of GA matrix:
\f[\hat{x}=(G_a^TC_g^{-1}G_a)^{-1}G_a^TC_g^{-1}\f]
This is very close to RR except replacing GX with GA.
We use the tomograhy parameters for lsr, since lsr is simply "tomography" onto DM directly.
*/
void setup_recon_lsr(RECON_T *recon, const PARMS_T *parms){
const int ndm=parms->ndm;
const int nwfs=parms->nwfsr;
cell *GAlsr;
cell *GAM=parms->recon.modal?(cell*)recon->GM:(cell*)recon->GA;
if(parms->recon.split){ //high order wfs only in split mode.
GAlsr=parms->recon.modal?(cell*)recon->GMhi:(cell*)recon->GAhi;
}else{ //all wfs in integrated mode.
GAlsr=GAM;
}
int free_GAlsr=0;
if(GAlsr->p[0]->id!=M_DBL){
dsp *tmp=dsp_cast(GAlsr->p[0]);
if(tmp->nzmax>tmp->nx*tmp->ny*0.2){//not very sparse
dcell *tmp2=0;
free_GAlsr=1;
dcelladd(&tmp2, 1, (dspcell*)GAlsr, 1);
GAlsr=(cell*)tmp2;
}
}
info2("Building recon->LR\n");
recon->LR.M=dcellmm2(GAlsr, recon->saneai, "tn");
// Tip/tilt and diff focus removal low rand terms for LGS WFS.
if(recon->TTF){
dcellmm(&recon->LR.U, recon->LR.M, recon->TTF, "nn", 1);
recon->LR.V=dcelltrans(recon->PTTF);
}
info2("Building recon->LL\n");
recon->LL.M=dcellmm2(recon->LR.M, GAlsr, "nn");
if(free_GAlsr){
cellfree(GAlsr);
}
double maxeig=pow(recon->neamhi * recon->aloc->p[0]->dx, -2);
if(parms->recon.modal){
double strength=1;
for(int idm=0; idm<ndm; idm++){
strength*=dnorm(recon->amod->p[idm]);
}
strength=pow(strength, 2./ndm);
maxeig*=strength;
}
if(fabs(parms->lsr.tikcr)>EPS){
info2("Adding tikhonov constraint of %g to LLM\n", parms->lsr.tikcr);
info2("The maximum eigen value is estimated to be around %g\n", maxeig);
dcelladdI(recon->LL.M, parms->lsr.tikcr*maxeig);
}
dcell *NW=NULL;
if(!parms->recon.modal){
if(parms->lsr.alg!=2){
/* Not SVD, need low rank terms for piston/waffle mode constraint. */
NW=dcellnew(ndm,1);
int nmod=2;/*two modes. */
for(int idm=0; idm<ndm; idm++){
loc_create_map(recon->aloc->p[idm]);
const long nloc=recon->aloc->p[idm]->nloc;
NW->p[idm]=dnew(nloc, ndm*nmod);
double *p=NW->p[idm]->p+nmod*idm*nloc;
const double *cpl=recon->actcpl->p[idm]->p;
for(long iloc=0; iloc<nloc; iloc++){
if(cpl[iloc]>0.1){
p[iloc]=1;/*piston mode */
}
}
/*notice offset of 1 because map start count at 1 */
p=NW->p[idm]->p+(1+nmod*idm)*nloc-1;
map_t *map=recon->aloc->p[idm]->map;
for(long iy=0; iy<map->ny; iy++){
for(long ix=0; ix<map->nx; ix++){
if(IND(map,ix,iy)){
p[(long)IND(map,ix,iy)]=(double)2*((iy+ix)&1)-1;
}
}
}
}
/*scale it to match the magnitude of LL.M */
dcellscale(NW, sqrt(maxeig));
if(parms->save.setup){
writebin(NW, "lsrNW");
}
}
if(parms->lsr.actslave){
/*actuator slaving. important. change from 0.5 to 0.1 on 2011-07-14. */
dspcell *actslave=slaving(recon->aloc, recon->actcpl, NW,
recon->actstuck, recon->actfloat, parms->lsr.actthres, maxeig);
if(parms->save.setup){
if(NW){
writebin(NW, "lsrNW2");
}
writebin(actslave,"actslave");
}
dcelladd(&recon->LL.M, 1, actslave, 1);
cellfree(actslave);
}
}
/*Low rank terms for low order wfs. Only in Integrated tomography. */
dcell *ULo=dcellnew(ndm,nwfs);
dcell *VLo=dcellnew(ndm,nwfs);
dcell* pULo=ULo/*PDELL*/;
dcell* pVLo=VLo/*PDELL*/;
for(int iwfs=0; iwfs<nwfs; iwfs++){
int ipowfs=parms->wfsr[iwfs].powfs;
if(parms->powfs[ipowfs].skip || !parms->powfs[ipowfs].lo){
continue;
}
for(int idm=0; idm<ndm; idm++){
dspfull(PIND(pULo,idm,iwfs), (dsp*)IND(recon->LR.M, idm, iwfs),'n',-1);
dspfull(PIND(pVLo,idm,iwfs), (dsp*)IND(GAM, iwfs, idm),'t',1);
}
}
recon->LL.U=dcellcat(recon->LR.U, ULo, 2);
dcell *GPTTDF=NULL;
dcellmm(&GPTTDF, GAM, recon->LR.V, "tn", 1);
recon->LL.V=dcellcat(GPTTDF, VLo, 2);
dcellfree(GPTTDF);
dcellfree(ULo);
dcellfree(VLo);
if(!parms->recon.modal && NW){
info2("Create piston and check board modes that are in NULL space of GA.\n");
/*add to low rank terms. */
dcell *tmp=recon->LL.U;
recon->LL.U=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellscale(NW, -1);
tmp=recon->LL.V;
recon->LL.V=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellfree(NW);
}
if(parms->lsr.fnreg){
warning("Loading LSR regularization from file %s.\n", parms->lsr.fnreg);
dspcell *tmp=dspcellread("%s", parms->lsr.fnreg);
dcelladd(&recon->LL.M, 1, tmp, 1);
dspcellfree(tmp);
}
recon->LL.alg = parms->lsr.alg;
recon->LL.bgs = parms->lsr.bgs;
recon->LL.warm = parms->recon.warm_restart;
recon->LL.maxit = parms->lsr.maxit;
/*Remove empty cells. */
dcelldropempty(&recon->LR.U,2);
dcelldropempty(&recon->LR.V,2);
dcelldropempty(&recon->LL.U,2);
dcelldropempty(&recon->LL.V,2);
if(parms->save.recon){
writebin(recon->LR.M,"LRM");
writebin(recon->LR.U,"LRU");
writebin(recon->LR.V,"LRV");
writebin(recon->LL.M,"LLM.bin");/*disable compression */
writebin(recon->LL.U,"LLU");
writebin(recon->LL.V,"LLV");
}
if(parms->lsr.alg==0 || parms->lsr.alg==2){
if(!parms->lsr.bgs){
muv_direct_prep(&recon->LL, (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
if(recon->LL.C)
chol_save(recon->LL.C, "LLC.bin");
else
writebin(recon->LL.MI, "LLMI.bin");
}
cellfree(recon->LL.M);
dcellfree(recon->LL.U);
dcellfree(recon->LL.V);
}else{
muv_direct_diag_prep(&(recon->LL), (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
for(int ib=0; ib<recon->LL.nb; ib++){
if(recon->LL.CB)
chol_save(recon->LL.CB[ib],"LLCB_%d.bin", ib);
else
writebin(recon->LL.MI,"LLMIB_%d.bin", ib);
}
}
/*Don't free M, U, V */
}
}
}
typedef struct {
char *name;
int type; /* 1 for read, 2 for write, 3 for r then write. */
int32 modes[10];
} OPS;
#define NUMOPS 56
static OPS optable[NUMOPS] =
{
{ "BRK", 0, { IMP(0x00), -1 } },
{ "RTI", 0, { IMP(0x40), -1 } },
{ "RTS", 0, { IMP(0x60), -1 } },
{ "PHA", 2, { IMP(0x48), -1 } },
{ "PHP", 2, { IMP(0x08), -1 } },
{ "PLA", 1, { IMP(0x68), -1 } },
{ "PLP", 1, { IMP(0x28), -1 } },
{ "JMP", 0, { ABS(0x4C), IND(0x6C), -1 } },
{ "JSR", 0, { ABS(0x20), -1 } },
{ "TAX", 0, { IMP(0xAA), -1 } },
{ "TXA", 0, { IMP(0x8A), -1 } },
{ "TAY", 0, { IMP(0xA8), -1 } },
{ "TYA", 0, { IMP(0x98), -1 } },
{ "TSX", 0, { IMP(0xBA), -1 } },
{ "TXS", 0, { IMP(0x9A), -1 } },
{ "DEX", 0, { IMP(0xCA), -1 } },
{ "DEY", 0, { IMP(0x88), -1 } },
{ "INX", 0, { IMP(0xE8), -1 } },
{ "INY", 0, { IMP(0xC8), -1 } },
{ "CLC", 0, { IMP(0x18), -1 } },
{ "CLD", 0, { IMP(0xD8), -1 } },
{ "CLI", 0, { IMP(0x58), -1 } },
{ "CLV", 0, { IMP(0xB8), -1 } },
int main()
{
START_MACHINE;
JUMP(CONTINUE);
#include "scheme.lib"
#include "char.lib"
#include "io.lib"
#include "math.lib"
#include "string.lib"
#include "system.lib"
CONTINUE:
/* Initialize stack with default values */
/* Void */
CALL(MAKE_SOB_VOID);
MOV(ADDR(10), IND(R0));
/* Nil (Empty List) */
CALL(MAKE_SOB_NIL);
MOV(ADDR(11), IND(R0));
/* False (Boolean) */
PUSH(IMM(0));
CALL(MAKE_SOB_BOOL);
MOV(ADDR(12), IND(R0));
MOV(ADDR(13), INDD(R0,1));
DROP(IMM(1));
/* True (Boolean) */
PUSH(IMM(1));
CALL(MAKE_SOB_BOOL);
MOV(ADDR(14), IND(R0));
MOV(ADDR(15), INDD(R0,1));
DROP(IMM(1));
/* END of initialization */
/* Internal function to check if a type is a SOB */
IS_SOB_TYPE:
MOV(R0, STARG(0));
MOV(R0, IND(R0));
CMP(R0, IMM(T_VOID));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_NIL));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_BOOL));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_CHAR));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_INTEGER));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_STRING));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_SYMBOL));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_PAIR));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_VECTOR));
JUMP_EQ(TRUE_SOB_TYPE);
CMP(R0, IMM(T_CLOSURE));
JUMP_EQ(TRUE_SOB_TYPE);
MOV(R0, IMM(0));
JUMP(EXIT_IS_SOB_TYPE);
TRUE_SOB_TYPE:
MOV(R0, IMM(1));
EXIT_IS_SOB_TYPE:
POP(FP);
RETURN;
/* End of IS_SOB_TYPE */
/* applic_3342061*/
/* applic_3342061 - B4 */
MOV(R0, IMM(12));
PUSH(R0);
/* applic_3342061 - B3 */
MOV(R0, IMM(14));
PUSH(R0);
/* applic_3342061 - B2 */
MOV(R0, IMM(12));
PUSH(R0);
/* applic_3342061 - B1 */
MOV(R0, IMM(14));
PUSH(R0);
PUSH(4);
PUSH(INDD(R0,1))
CALL(INDD(R0,2))
MOV(IND(R1),IMM(2));
ADD(IND(R1),IMM(4));
MOV(SP,IND(R1));
PUSH(R0);
CALL(WRITE_SOB);
STOP_MACHINE;
return 0;
}
/**
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 */
}
