- for (_j = beg; _j < ma->n; ++_j) {
- int k, j = _j - beg, _min = last_min, _max = last_max;
- double p[3], sum;
- pdg = ma->pdg + _j * 3;
- p[0] = pdg[0]; p[1] = 2. * pdg[1]; p[2] = pdg[2];
- for (; _min < _max && z[0][_min] < TINY; ++_min) z[0][_min] = z[1][_min] = 0.;
- for (; _max > _min && z[0][_max] < TINY; --_max) z[0][_max] = z[1][_max] = 0.;
- _max += 2;
- if (_min == 0)
- k = 0, z[1][k] = (2*j+2-k)*(2*j-k+1) * p[0] * z[0][k];
- if (_min <= 1)
- k = 1, z[1][k] = (2*j+2-k)*(2*j-k+1) * p[0] * z[0][k] + k*(2*j+2-k) * p[1] * z[0][k-1];
- for (k = _min < 2? 2 : _min; k <= _max; ++k)
- z[1][k] = (2*j+2-k)*(2*j-k+1) * p[0] * z[0][k]
- + k*(2*j+2-k) * p[1] * z[0][k-1]
- + k*(k-1)* p[2] * z[0][k-2];
- for (k = _min, sum = 0.; k <= _max; ++k) sum += z[1][k];
- ma->t += log(sum / ((2. * j + 2) * (2. * j + 1)));
- for (k = _min; k <= _max; ++k) z[1][k] /= sum;
- if (_min >= 1) z[1][_min-1] = 0.;
- if (_min >= 2) z[1][_min-2] = 0.;
- if (j < ma->n - 1) z[1][_max+1] = z[1][_max+2] = 0.;
- if (_j == ma->n1 - 1) { // set pop1
- ma->t1 = ma->t;
- memcpy(ma->z1, z[1], sizeof(double) * (ma->n1 * 2 + 1));
+ if (ma->M == ma->n * 2) {
+ int M = 0;
+ for (_j = beg; _j < ma->n; ++_j) {
+ int k, j = _j - beg, _min = last_min, _max = last_max, M0;
+ double p[3], sum;
+ M0 = M; M += 2;
+ pdg = ma->pdg + _j * 3;
+ p[0] = pdg[0]; p[1] = 2. * pdg[1]; p[2] = pdg[2];
+ for (; _min < _max && z[0][_min] < TINY; ++_min) z[0][_min] = z[1][_min] = 0.;
+ for (; _max > _min && z[0][_max] < TINY; --_max) z[0][_max] = z[1][_max] = 0.;
+ _max += 2;
+ if (_min == 0) k = 0, z[1][k] = (M0-k+1) * (M0-k+2) * p[0] * z[0][k];
+ if (_min <= 1) k = 1, z[1][k] = (M0-k+1) * (M0-k+2) * p[0] * z[0][k] + k*(M0-k+2) * p[1] * z[0][k-1];
+ for (k = _min < 2? 2 : _min; k <= _max; ++k)
+ z[1][k] = (M0-k+1)*(M0-k+2) * p[0] * z[0][k] + k*(M0-k+2) * p[1] * z[0][k-1] + k*(k-1)* p[2] * z[0][k-2];
+ for (k = _min, sum = 0.; k <= _max; ++k) sum += z[1][k];
+ ma->t += log(sum / (M * (M - 1.)));
+ for (k = _min; k <= _max; ++k) z[1][k] /= sum;
+ if (_min >= 1) z[1][_min-1] = 0.;
+ if (_min >= 2) z[1][_min-2] = 0.;
+ if (j < ma->n - 1) z[1][_max+1] = z[1][_max+2] = 0.;
+ if (_j == ma->n1 - 1) { // set pop1; ma->n1==-1 when unset
+ ma->t1 = ma->t;
+ memcpy(ma->z1, z[1], sizeof(double) * (ma->n1 * 2 + 1));
+ }
+ tmp = z[0]; z[0] = z[1]; z[1] = tmp;
+ last_min = _min; last_max = _max;
+ }
+ //for (_j = 0; _j < last_min; ++_j) z[0][_j] = 0.; // TODO: are these necessary?
+ //for (_j = last_max + 1; _j < ma->M; ++_j) z[0][_j] = 0.;
+ } else { // this block is very similar to the block above; these two might be merged in future
+ int j, M = 0;
+ for (j = 0; j < ma->n; ++j) {
+ int k, M0, _min = last_min, _max = last_max;
+ double p[3], sum;
+ pdg = ma->pdg + j * 3;
+ for (; _min < _max && z[0][_min] < TINY; ++_min) z[0][_min] = z[1][_min] = 0.;
+ for (; _max > _min && z[0][_max] < TINY; --_max) z[0][_max] = z[1][_max] = 0.;
+ M0 = M;
+ M += ma->ploidy[j];
+ if (ma->ploidy[j] == 1) {
+ p[0] = pdg[0]; p[1] = pdg[2];
+ _max++;
+ if (_min == 0) k = 0, z[1][k] = (M0+1-k) * p[0] * z[0][k];
+ for (k = _min < 1? 1 : _min; k <= _max; ++k)
+ z[1][k] = (M0+1-k) * p[0] * z[0][k] + k * p[1] * z[0][k-1];
+ for (k = _min, sum = 0.; k <= _max; ++k) sum += z[1][k];
+ ma->t += log(sum / M);
+ for (k = _min; k <= _max; ++k) z[1][k] /= sum;
+ if (_min >= 1) z[1][_min-1] = 0.;
+ if (j < ma->n - 1) z[1][_max+1] = 0.;
+ } else if (ma->ploidy[j] == 2) {
+ p[0] = pdg[0]; p[1] = 2 * pdg[1]; p[2] = pdg[2];
+ _max += 2;
+ if (_min == 0) k = 0, z[1][k] = (M0-k+1) * (M0-k+2) * p[0] * z[0][k];
+ if (_min <= 1) k = 1, z[1][k] = (M0-k+1) * (M0-k+2) * p[0] * z[0][k] + k*(M0-k+2) * p[1] * z[0][k-1];
+ for (k = _min < 2? 2 : _min; k <= _max; ++k)
+ z[1][k] = (M0-k+1)*(M0-k+2) * p[0] * z[0][k] + k*(M0-k+2) * p[1] * z[0][k-1] + k*(k-1)* p[2] * z[0][k-2];
+ for (k = _min, sum = 0.; k <= _max; ++k) sum += z[1][k];
+ ma->t += log(sum / (M * (M - 1.)));
+ for (k = _min; k <= _max; ++k) z[1][k] /= sum;
+ if (_min >= 1) z[1][_min-1] = 0.;
+ if (_min >= 2) z[1][_min-2] = 0.;
+ if (j < ma->n - 1) z[1][_max+1] = z[1][_max+2] = 0.;
+ }
+ tmp = z[0]; z[0] = z[1]; z[1] = tmp;
+ last_min = _min; last_max = _max;