Fill in default upper & lower bounds
[openmx:openmx.git] / src / libifa-rpf.c
1 /*
2   Copyright 2012-2013 Joshua Nathaniel Pritikin and contributors
3
4   libifa-rpf is free software: you can redistribute it and/or modify
5   it under the terms of the GNU General Public License as published by
6   the Free Software Foundation, either version 3 of the License, or
7   (at your option) any later version.
8
9   This program is distributed in the hope that it will be useful,
10   but WITHOUT ANY WARRANTY; without even the implied warranty of
11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12   GNU General Public License for more details.
13
14   You should have received a copy of the GNU General Public License
15   along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include <R.h>
19 #include <R_ext/BLAS.h>
20 #include <stdlib.h>
21 #include <math.h>
22 #include <string.h>
23 #include "libifa-rpf.h"
24
25 #ifndef M_LN2
26 #define M_LN2           0.693147180559945309417232121458        /* ln(2) */
27 #endif
28
29 #ifndef M_LN_SQRT_PI
30 #define M_LN_SQRT_PI    0.572364942924700087071713675677        /* log(sqrt(pi))
31                                                                    == log(pi)/2 */
32 #endif
33
34 static const double EXP_STABLE_DOMAIN = 35;
35
36 // This is far away from the item's difficulty so it is less
37 // interesting for estimation and the gradient becomes numerically
38 // unstable as athb < -25. For symmetry, it is necessary to clamp at
39 // 25 as well.
40 static const double GRADIENT_STABLE_DOMAIN = 25;
41
42 static void
43 irt_rpf_logprob_adapter(const double *spec,
44                         const double *restrict param, const double *restrict th,
45                         double *restrict out)
46 {
47   (*librpf_model[(int) spec[RPF_ISpecID]].prob)(spec, param, th, out);
48
49   int numOutcomes = spec[RPF_ISpecOutcomes];
50   for (int ox=0; ox < numOutcomes; ox++) {
51     out[ox] = log(out[ox]);
52   }
53 }
54
55 static double
56 dotprod(const double *v1, const double *v2, const int len)
57 {
58   double dprod = 0;
59   for (int dx=0; dx < len; dx++) {
60     dprod += v1[dx] * v2[dx];
61   }
62   return dprod;
63 }
64
65 static int
66 hessianIndex(int numParam, int row, int col)
67 {
68   return numParam + row*(row+1)/2 + col;
69 }
70
71 /**
72  * lognormal(x,sd) := (x*sd*sqrt(2*%pi))^-1 * exp(-(log(x)^2)/(2*sd^2))
73  * log(lognormal(x,sd)), logexpand=super;
74  */
75 static double lognormal_pdf(double aa, double sd)
76 {
77   double sd2 = sd*sd;
78   double loga = log(aa);
79   return -loga*loga/(2*sd2) - loga - log(sd) - M_LN_SQRT_PI - M_LN2/2;
80 }
81
82 /**
83  * diff(log(lognormal(a,sd))),a), logexpand=super;
84  */
85 static double lognormal_gradient(double aa, double sd)
86 {
87   double sd2 = sd*sd;
88   return -log(aa)/(aa * sd2) - 1/aa;
89 }
90
91 static double logit(double prob)
92 {
93   return log(prob/(1-prob));
94 }
95
96 // Prior for the lower asymptote parameter (Cai, Yang & Hansen, 2011, p. 246)
97
98 /**
99  * normal(x,mean,sd) := (sd*sqrt(2*%pi))^-1 * exp(-((x - mean)^2)/(2*sd^2))
100  * log(normal(log(x/(1-x)),mean,sd)), logexpand=super;
101  */
102 static double logitnormal_pdf(double cc, double mean, double sd)
103 {
104   double sd2 = sd * sd;
105   double offset = logit(cc) - mean;
106   return -(offset*offset)/(2*sd2) - log(sd) - M_LN_SQRT_PI - M_LN2/2;
107 }
108
109 /**
110  * factor(diff(log(normal(log(x/(1-x)),mean,sd)),x))
111  */
112 static double logitnormal_gradient(double cc, double mean, double sd)
113 {
114   double sd2 = sd * sd;
115   return (logit(cc) - mean) / (sd2 * (cc-1) * cc);
116 }
117
118 static int
119 irt_rpf_1dim_drm_numSpec(const double *spec)
120 { return RPF_ISpecCount; }
121
122 static int
123 irt_rpf_1dim_drm_numParam(const double *spec)
124 { return 4; }
125
126 static void
127 irt_rpf_1dim_drm_prob(const double *spec,
128                       const double *restrict param, const double *restrict th,
129                       double *restrict out)
130 {
131   double guessing = param[2];
132   double upper = param[3];
133   double athb = -param[0] * (th[0] - param[1]);
134   if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
135   else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
136   double pp = guessing + (upper-guessing) / (1 + exp(athb));
137   out[0] = 1-pp;
138   out[1] = pp;
139 }
140
141 static void
142 set_deriv_nan(const double *spec, double *out)
143 {
144   int numParam = (*librpf_model[(int) spec[RPF_ISpecID]].numParam)(spec);
145
146   for (int px=0; px < numParam; px++) {
147     out[px] = nan("I");
148   }
149 }
150
151 /**
152  * irf(a,b,c,th) := c+(1-c)/(1+exp(-a*(th - b)))
153  * diff(log(1-irf(a,b,c,th)),a);  // 0
154  * diff(log(irf(a,b,c,th)),a);    // 1
155  * diff(log(1-irf(a,b,c,th)),b);  // 0
156  * diff(log(irf(a,b,c,th)),b);    // 1
157  * ratsimp(diff(log(1-irf(a,b,c,th)),c));
158  * diff(log(irf(a,b,c,th)),c);
159  */
160 static void
161 irt_rpf_1dim_drm_deriv1(const double *spec,
162                         const double *restrict param,
163                         const double *where, const double area,
164                         const double *weight, double *out)
165 {
166   double aa = param[0];
167   double bb = param[1];
168   double cc = param[2];
169   double th = where[0];
170   double athb = -aa * (th - bb);
171
172   if (athb < -GRADIENT_STABLE_DOMAIN) athb = -GRADIENT_STABLE_DOMAIN;
173   else if (athb > GRADIENT_STABLE_DOMAIN) athb = GRADIENT_STABLE_DOMAIN;
174
175   double Eathb = exp(athb);
176   double Eathb2 = (Eathb+1)*(Eathb+1);
177   double w0 = Eathb2 * (-(1-cc)/(Eathb+1) - cc + 1);
178   double w1 = Eathb2 * ((1-cc)/(Eathb+1) + cc);
179
180   out[0] += area * ((weight[0] * (bb-th)*Eathb / w0 +
181                      weight[1] * -(bb-th)*Eathb / w1));
182   out[1] += area * ((weight[0] * Eathb / w0 +
183                      weight[1] * -Eathb / w1));
184   double ratio = (1-(1/(Eathb+1))) / ((1-cc)/(Eathb+1) + cc);
185   out[2] += area * ((weight[0] * (1/(cc-1)) +
186                      weight[1] * ratio));
187 }
188
189 static void
190 irt_rpf_1dim_drm_deriv2(const double *spec, const double *restrict param,
191                         double *out)
192 {
193   double aa = param[0];
194   double cc = param[2];
195   const double *prior = spec + RPF_ISpecCount;
196   if (aa <= 0 || cc < 0 || cc >= 1) {
197     set_deriv_nan(spec, out);
198     return;
199   }
200
201   out[0] *= (1-cc);
202   //out[0] += lognormal_gradient(aa, prior[0]);
203
204   out[1] *= aa * (1-cc);
205   if (cc == 0) { out[2] = nan("I"); }
206   else {
207     out[2] += logitnormal_gradient(cc, prior[1], prior[2]);
208   }
209   for (int px=3; px < 9; px++) out[px] = nan("I");
210 }
211
212 static void
213 irt_rpf_1dim_drm_rescale(const double *spec, double *restrict param, const int *paramMask,
214                          const double *restrict mean, const double *restrict choleskyCov)
215 {
216   double thresh = param[1] * -param[0];
217   if (paramMask[0] >= 0) {
218     param[0] *= choleskyCov[0];
219   }
220   if (paramMask[1] >= 0) {
221     thresh += param[0] * mean[0];
222     param[1] = thresh / -param[0];
223   }
224 }
225
226 static int
227 irt_rpf_mdim_drm_numSpec(const double *spec)
228 { return RPF_ISpecCount; }
229
230 static int
231 irt_rpf_mdim_drm_numParam(const double *spec)
232 { return 3 + spec[RPF_ISpecDims]; }
233
234 static void
235 irt_rpf_mdim_drm_paramBound(const double *spec, const int param,
236                             double *upper, double *lower)
237 {
238         int numDims = spec[RPF_ISpecDims];
239         *upper = nan("unset");
240         *lower = nan("unset");
241         if (param >= 0 && param < numDims) {
242                 *lower = 1e-6;
243         } else if (param == numDims+1 || param == numDims+2) {
244                 *lower = 1e-6;
245                 *upper = 1 - 1e-6;
246         }
247 }
248
249 static void
250 irt_rpf_mdim_drm_prob(const double *spec,
251                       const double *restrict param, const double *restrict th,
252                       double *restrict out)
253 {
254   int numDims = spec[RPF_ISpecDims];
255   double dprod = dotprod(param, th, numDims);
256   double diff = param[numDims];
257   double guessing = param[numDims+1];
258   double upper = param[numDims+2];
259   double athb = -(dprod + diff);
260   if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
261   else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
262   double tmp = guessing + (upper-guessing) / (1 + exp(athb));
263   out[0] = 1-tmp;
264   out[1] = tmp;
265 }
266
267 static void
268 irt_rpf_mdim_drm_prob2(const double *spec,
269                        const double *restrict param, const double *restrict th,
270                        double *restrict out1, double *restrict out2)
271 {
272   int numDims = spec[RPF_ISpecDims];
273   double dprod = dotprod(param, th, numDims);
274   double diff = param[numDims];
275   double guessing = param[numDims+1];
276   double upper = param[numDims+2];
277   double athb = -(dprod + diff);
278   if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
279   else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
280   double tmp = 1 / (1 + exp(athb));
281   out1[0] = 1-tmp;
282   out1[1] = tmp;
283   tmp = guessing + (upper-guessing) * tmp;
284   out2[0] = 1-tmp;
285   out2[1] = tmp;
286 }
287
288 static void
289 irt_rpf_mdim_drm_deriv1(const double *spec,
290                        const double *restrict param,
291                        const double *where, const double area,
292                        const double *weight, double *out)
293 {
294   int numDims = spec[RPF_ISpecDims];
295   double cc = param[numDims+1];
296   double upper = param[numDims+2];
297   double PQ[2];
298   double PQstar[2];
299   irt_rpf_mdim_drm_prob2(spec, param, where, PQstar, PQ);
300   double r1 = weight[0];
301   double r2 = weight[1];
302   double r1_P = r1/PQ[0];
303   double r1_P2 = r1/(PQ[0] * PQ[0]);
304   double r2_Q = r2/PQ[1];
305   double r2_Q2 = r2/(PQ[1] * PQ[1]);
306   for (int dx=0; dx < numDims; dx++) {
307     out[dx] -= area * where[dx] * PQstar[0] * PQstar[1] * (upper-cc) * (r1_P - r2_Q);
308   }
309   out[numDims] -= area * (upper-cc) * PQstar[0] * PQstar[1] * (r1_P - r2_Q);
310   out[numDims+1] -= area * PQstar[0] * (r1_P - r2_Q);
311   out[numDims+2] -= area * PQstar[1] * (r1_P - r2_Q);
312
313   int ox = numDims+2;
314   double ugD2 = (upper-cc);  // can factor out TODO
315   double ugD = (upper-cc);
316   double Pstar = PQstar[0];
317   double Pstar2 = Pstar * Pstar;
318   double Pstar3 = Pstar2 * Pstar;
319   double Qstar = PQstar[1];
320   double Qstar2 = Qstar * Qstar;
321
322   for(int ix=0; ix < numDims; ix++) {
323     for(int jx=0; jx <= ix; jx++) {
324       out[++ox] += area * (r1_P * (ugD2 * where[ix] * where[jx] *
325                                    (Pstar - 3*Pstar2 + 2*Pstar3)) -
326                            r1_P2 * (ugD * where[ix] * (Pstar - Pstar2) *
327                                     (ugD * where[jx] * (Pstar - Pstar2))) +
328                            r2_Q * (ugD2 * where[ix] * where[jx] *
329                                    (-Pstar + 3*Pstar2 - 2*Pstar3)) -
330                            r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) *
331                                     (ugD * where[jx] * (-Pstar + Pstar2))));
332     }
333   }
334   for(int ix=0; ix < numDims; ix++) {
335     out[++ox] += area * (r1_P * (ugD2 * where[ix] * (Pstar - 3*Pstar2 + 2*Pstar3)) -
336                          r1_P2 * (ugD * where[ix] * (Pstar - Pstar2) *
337                                   (ugD * (Pstar - Pstar2))) +
338                          r2_Q * (ugD2 * where[ix] * (-Pstar + 3*Pstar2 - 2*Pstar3)) -
339                          r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) *
340                                   (ugD * (-Pstar + Pstar2))));
341   }
342   double chunk1 = ugD * (Pstar - Pstar2);
343   out[++ox] += area * (r1_P * (ugD2 * (Pstar - 3*Pstar2 + 2*Pstar3)) -
344                        r1_P2 * chunk1*chunk1 +
345                        r2_Q * (ugD2 * (-Pstar + 3*Pstar2 - 2*Pstar3)) -
346                        r2_Q2 * chunk1*chunk1);
347   for(int ix=0; ix < numDims; ix++) {
348     out[++ox] += area * (r1_P * (where[ix] * (Pstar - Pstar2)) -
349                          r1_P2 * (ugD * where[ix] * (Pstar - Pstar2)) * Pstar +
350                          r2_Q * (where[ix] * (-Pstar + Pstar2)) +
351                          r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) ) * Pstar);
352   }
353   out[++ox] += area * (r1_P * (Pstar - Pstar2) -
354                        r1_P2 * (ugD * (Pstar - Pstar2)) * Pstar +
355                        r2_Q * (-Pstar + Pstar2) +
356                        r2_Q2 * (ugD * (-Pstar + Pstar2)) * Pstar);
357   out[++ox] -= area * Pstar2 * (r1_P2 + r2_Q2);
358
359   for(int ix=0; ix < numDims; ix++) {
360     out[++ox] += area * (r1_P * (where[ix] * (-Pstar + Pstar2)) -
361                          r1_P2 * (ugD * where[ix] * (Pstar - Pstar2)) * Qstar +
362                          r2_Q * (where[ix] * (Pstar - Pstar2)) -
363                          r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) ) * (-1 + Pstar));
364   }
365
366   out[++ox] += area * (r1_P * (-Pstar + Pstar2) -
367                        r1_P2 * (ugD * (Pstar - Pstar2)) * Qstar +
368                        r2_Q * (Pstar - Pstar2) -
369                        r2_Q2 * (ugD * (-Pstar + Pstar2)) * -Qstar);
370
371   out[++ox] += area * (-r1_P2 * Pstar * Qstar + r2_Q2 * Pstar * (-1 + Pstar));
372   out[++ox] -= area * Qstar2 * (r1_P2 + r2_Q2);
373 }
374
375 static void
376 irt_rpf_mdim_drm_deriv2(const double *spec,
377                         const double *restrict param,
378                         double *out)
379 {
380   int numDims = spec[RPF_ISpecDims];
381   const double *aa = param;
382   double cc = param[numDims+1];
383   double upper = param[numDims+2];
384
385   if (cc < 0 || cc >= 1 || upper <= 0 || upper > 1) {
386     set_deriv_nan(spec, out);
387     return;
388   }
389   for (int dx=0; dx < numDims; dx++) {
390     if (aa[dx] < 0) {
391       set_deriv_nan(spec, out);
392       return;
393     }
394   }
395   if (cc == 0) {
396     out[numDims+1] = nan("I");
397   }
398   if (upper == 1) {
399     out[numDims+2] = nan("I");
400   }
401 }
402
403 static void
404 irt_rpf_mdim_drm_rescale(const double *spec, double *restrict param, const int *paramMask,
405                          const double *restrict mean, const double *restrict choleskyCov)
406 {
407   int numDims = spec[RPF_ISpecDims];
408
409   for (int d1=0; d1 < numDims; d1++) {
410     if (paramMask[d1] < 0) continue;
411     param[d1] = dotprod(param+d1, choleskyCov + d1 * numDims + d1, numDims-d1);
412   }
413
414   double madj = dotprod(param, mean, numDims);
415
416   param[numDims] += madj;
417 }
418
419 static void
420 irt_rpf_mdim_drm_dTheta(const double *spec, const double *restrict param,
421                         const double *where, const double *dir,
422                         double *grad, double *hess)
423 {
424   int numDims = spec[RPF_ISpecDims];
425   double PQ[2];
426   double PQstar[2];
427   irt_rpf_mdim_drm_prob2(spec, param, where, PQstar, PQ);
428   double Pstar = PQstar[0];
429   double Qstar = PQstar[1];
430   const double *aa = param;
431   const double guess = param[numDims + 1];
432   const double upper = param[numDims + 2];
433   for (int ax=0; ax < numDims; ax++) {
434     double piece = dir[ax] * (upper-guess) * aa[ax] * (Pstar * Qstar);
435     grad[1] += piece;
436     grad[0] -= piece;
437     piece = dir[ax] * (2 * (upper - guess) * aa[ax]*aa[ax] * (Qstar * Qstar * Pstar) -
438                        (upper - guess) * aa[ax]*aa[ax] * (Pstar * Qstar));
439     hess[1] -= piece;
440     hess[0] += piece;
441   }
442 }
443
444 static void
445 irt_rpf_1dim_drm_dTheta(const double *spec, const double *restrict param,
446                         const double *where, const double *dir,
447                         double *grad, double *hess)
448 {
449   double nparam[4];
450   memcpy(nparam, param, sizeof(double) * 4);
451   nparam[1] = param[1] * -param[0];
452   irt_rpf_mdim_drm_dTheta(spec, nparam, where, dir, grad, hess);
453 }
454
455 static void
456 irt_rpf_1dim_grm_prob(const double *spec,
457                       const double *restrict param, const double *restrict th,
458                       double *restrict out)
459 {
460   const int numOutcomes = spec[RPF_ISpecOutcomes];
461   const double slope = param[0];
462   const double theta = th[0];
463   const double *kat = param + (int) spec[RPF_ISpecDims];
464
465   double athb = -slope * (theta - kat[0]);
466   if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
467   else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
468   double tmp = 1 / (1 + exp(athb));
469   out[0] = 1-tmp;
470   out[1] = tmp;
471
472   for (int kx=2; kx < numOutcomes; kx++) {
473     double athb = -slope * (theta - kat[kx-1]);
474     if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
475     else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
476     double tmp = 1 / (1 + exp(athb));
477     out[kx-1] -= tmp;
478     out[kx] = tmp;
479   }
480 }
481
482 static int
483 irt_rpf_mdim_grm_numSpec(const double *spec)
484 { return RPF_ISpecCount; }
485
486 static int
487 irt_rpf_mdim_grm_numParam(const double *spec)
488 { return spec[RPF_ISpecOutcomes] + spec[RPF_ISpecDims] - 1; }
489
490 static void
491 irt_rpf_mdim_grm_paramBound(const double *spec, const int param,
492                             double *upper, double *lower)
493 {
494         int numDims = spec[RPF_ISpecDims];
495         const int numOutcomes = spec[RPF_ISpecOutcomes];
496         *upper = nan("unset");
497         *lower = nan("unset");
498         if (param >= 0 && param < numDims) {
499                 *lower = 1e-6;
500         }
501 }
502
503 static void
504 irt_rpf_mdim_grm_prob(const double *spec,
505                       const double *restrict param, const double *restrict th,
506                       double *restrict out)
507 {
508   const int numDims = spec[RPF_ISpecDims];
509   const int numOutcomes = spec[RPF_ISpecOutcomes];
510   const double *slope = param;
511   const double dprod = dotprod(slope, th, numDims);
512   const double *kat = param + (int) spec[RPF_ISpecDims];
513
514   double athb = -(dprod + kat[0]);
515   if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
516   else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
517   double tmp = 1 / (1 + exp(athb));
518   out[0] = 1-tmp;
519   out[1] = tmp;
520
521   for (int kx=2; kx < numOutcomes; kx++) {
522     double athb = -(dprod + kat[kx-1]);
523     if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
524     else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
525     double tmp = 1 / (1 + exp(athb));
526     out[kx-1] -= tmp;
527     out[kx] = tmp;
528   }
529
530   // look for crazy stuff
531   for (int kx=0; kx < numOutcomes; kx++) {
532     if (out[kx] <= 0) {
533       int bigk = -1;
534       double big = 0;
535       for (int bx=0; bx < numOutcomes; bx++) {
536         if (out[bx] > big) {
537           bigk = bx;
538           big = out[bx];
539         }
540       }
541       for (int fx=0; fx < numOutcomes; fx++) {
542         if (out[fx] < 0) error("GRM categories are out of order");
543         if (out[fx] == 0) {
544           double small = 1 / (1 + exp(EXP_STABLE_DOMAIN));
545           out[bigk] -= small;
546           out[fx] += small;
547         }
548       }
549       return;
550     }
551   }
552 }
553
554 static void
555 irt_rpf_mdim_grm_rawprob(const double *spec,
556                          const double *restrict param, const double *restrict th,
557                          double *restrict out)
558 {
559   int numDims = spec[RPF_ISpecDims];
560   const int numOutcomes = spec[RPF_ISpecOutcomes];
561   const double dprod = dotprod(param, th, numDims);
562   const double *kat = param + (int) spec[RPF_ISpecDims];
563
564   out[0] = 1;
565   for (int kx=0; kx < numOutcomes-1; kx++) {
566     double athb = -(dprod + kat[kx]);
567     if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
568     else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
569     double tmp = 1 / (1 + exp(athb));
570     out[kx+1] = tmp;
571   }
572   out[numOutcomes] = 0;
573 }
574
575 static void
576 irt_rpf_mdim_grm_deriv1(const double *spec,
577                         const double *restrict param,
578                         const double *where, const double area,
579                         const double *weight, double *out)
580 {
581   int nfact = spec[RPF_ISpecDims];
582   int outcomes = spec[RPF_ISpecOutcomes];
583   int nzeta = spec[RPF_ISpecOutcomes] - 1;
584   double P[nzeta+2];
585   double PQfull[nzeta+2];
586   irt_rpf_mdim_grm_rawprob(spec, param, where, P);
587   PQfull[0] = 0;
588   PQfull[outcomes] = 0;
589   for (int kx=1; kx <= nzeta; kx++) PQfull[kx] = P[kx] * (1-P[kx]);
590   for (int jx = 0; jx <= nzeta; jx++) {
591     double Pk_1 = P[jx];
592     double Pk = P[jx + 1];
593     double PQ_1 = PQfull[jx];
594     double PQ = PQfull[jx + 1];
595     double Pk_1Pk = Pk_1 - Pk;
596     if (Pk_1Pk < 1e-10) Pk_1Pk = 1e-10;
597     double dif1 = weight[jx] / Pk_1Pk;
598     double dif1sq = weight[jx] / (Pk_1Pk * Pk_1Pk);
599     if(jx < nzeta) {
600       double Pk_p1 = P[jx + 2];
601       double PQ_p1 = PQfull[jx + 2];
602       double Pk_Pkp1 = Pk - Pk_p1;
603       if(Pk_Pkp1 < 1e-10) Pk_Pkp1 = 1e-10;
604       double dif2 = weight[jx+1] / Pk_Pkp1;
605       double dif2sq = weight[jx+1] / (Pk_Pkp1 * Pk_Pkp1);
606       out[nfact + jx] -= area * PQ * (dif1 - dif2);
607
608       int d2base = (nfact + nzeta) + (nfact+jx) * (nfact+jx+1)/2;
609       out[d2base + nfact + jx] += area * (-1 * PQ * PQ * (dif1sq + dif2sq) -
610                                           (dif1 - dif2) * (Pk * (1.0 - Pk) * (1.0 - 2.0*Pk)));
611       if (jx < (nzeta - 1)) {
612         int d2base1 = (nfact + nzeta) + (nfact+jx+1) * (nfact+jx+2)/2;
613         out[d2base1 + nfact + jx] += area * dif2sq * PQ_p1 * PQ;
614       }
615       double tmp1 = (-1.0) * dif2sq * PQ * (PQ - PQ_p1);
616       double tmp2 = dif1sq * PQ * (PQ_1 - PQ);
617       double tmp3 = (dif1 - dif2) * (Pk * (1.0 - Pk) * (1.0 - 2.0*Pk));
618       for(int kx = 0; kx < nfact; kx++){
619         out[d2base + kx] += area * (tmp1 + tmp2 - tmp3) * where[kx];
620       }
621     }
622     for(int kx = 0; kx < nfact; kx++) {
623       out[kx] += area * dif1 * (PQ_1 - PQ) * where[kx];
624     }
625
626     double temp[nfact];
627     for(int ix = 0; ix < nfact; ix++)
628       temp[ix] = PQ_1 * where[ix] - PQ * where[ix];
629
630     int d2x = nfact + nzeta;
631     for(int i = 0; i < nfact; i++) {
632       for(int j = 0; j <= i; j++) {
633         double outer = where[i]*where[j];
634         out[d2x++] += area * (-1 * dif1sq * temp[i] * temp[j] +
635                               (dif1 * (Pk_1 * (1.0 - Pk_1) * (1.0 - 2.0 * Pk_1) * 
636                                        outer - Pk * (1.0 - Pk) * (1.0 - 2.0 * Pk) * outer)));
637       }
638     }
639   }
640 }
641
642 static void
643 irt_rpf_mdim_grm_deriv2(const double *spec,
644                         const double *restrict param,
645                         double *out)
646 {
647   int nfact = spec[RPF_ISpecDims];
648   int nzeta = spec[RPF_ISpecOutcomes] - 1;
649   const double *aa = param;
650   for (int dx=0; dx < nfact; dx++) {
651     if (aa[dx] < 0) {
652       set_deriv_nan(spec, out);
653       return;
654     }
655   }
656   for (int zx=0; zx < nzeta-1; zx++) {
657     if (param[nfact+zx] < param[nfact+zx+1]) {
658       set_deriv_nan(spec, out);
659       return;
660     }
661   }
662 }
663
664 static void
665 irt_rpf_mdim_grm_dTheta(const double *spec, const double *restrict param,
666                         const double *where, const double *dir,
667                         double *grad, double *hess)
668 {
669   int numDims = spec[RPF_ISpecDims];
670   int outcomes = spec[RPF_ISpecOutcomes];
671   const double *aa = param;
672   double P[outcomes+1];
673   irt_rpf_mdim_grm_rawprob(spec, param, where, P);
674   for (int jx=0; jx < numDims; jx++) {
675     for (int ix=0; ix < outcomes; ix++) {
676       double w1 = P[ix] * (1-P[ix]) * aa[jx];
677       double w2 = P[ix+1] * (1-P[ix+1]) * aa[jx];
678       grad[ix] += dir[jx] * (w1 - w2);
679       hess[ix] += dir[jx] * (aa[jx]*aa[jx] * (2 * P[ix] * (1 - P[ix])*(1 - P[ix]) -
680                                               P[ix] * (1 - P[ix]) -
681                                               2 * P[ix+1] * (1 - P[ix+1])*(1 - P[ix+1]) +
682                                               P[ix+1] * (1 - P[ix+1])));
683     }
684   }
685 }
686
687 static void
688 irt_rpf_mdim_grm_rescale(const double *spec, double *restrict param, const int *paramMask,
689                          const double *restrict mean, const double *restrict choleskyCov)
690 {
691   int numDims = spec[RPF_ISpecDims];
692   int nzeta = spec[RPF_ISpecOutcomes] - 1;
693
694   for (int d1=0; d1 < numDims; d1++) {
695     if (paramMask[d1] < 0) continue;
696     param[d1] = dotprod(param+d1, choleskyCov + d1 * numDims + d1, numDims-d1);
697   }
698
699   double madj = dotprod(param, mean, numDims);
700
701   for (int tx=0; tx < nzeta; tx++) {
702     int px = numDims + tx;
703     if (paramMask[px] >= 0) param[px] += madj;
704   }
705 }
706
707 static int
708 irt_rpf_nominal_numSpec(const double *spec)
709 {
710   int outcomes = spec[RPF_ISpecOutcomes];
711   int Tlen = (outcomes - 1) * (outcomes - 1);
712   return RPF_ISpecCount + 4 * Tlen;
713 }
714
715 static int
716 irt_rpf_nominal_numParam(const double *spec)
717 { return spec[RPF_ISpecDims] + 2 * (spec[RPF_ISpecOutcomes]-1); }
718
719
720 static void
721 irt_rpf_nominal_paramBound(const double *spec, const int param,
722                            double *upper, double *lower)
723 {
724         int numDims = spec[RPF_ISpecDims];
725         const int numOutcomes = spec[RPF_ISpecOutcomes];
726         *upper = nan("unset");
727         *lower = nan("unset");
728         if (param >= 0 && param < numDims) {
729                 *lower = 1e-6;
730         }
731 }
732
733 static void
734 _nominal_rawprob(const double *spec,
735                  const double *restrict param, const double *restrict th,
736                  double discr, double *ak, double *num)
737 {
738   int numDims = spec[RPF_ISpecDims];
739   int numOutcomes = spec[RPF_ISpecOutcomes];
740   const double *alpha = param + numDims;
741   const double *gamma = param + numDims + numOutcomes - 1;
742   const double *Ta = spec + RPF_ISpecCount;
743   const double *Tc = spec + RPF_ISpecCount + (numOutcomes-1) * (numOutcomes-1);
744
745   for (int kx=0; kx < numOutcomes; kx++) {
746     ak[kx] = 0;
747     double ck = 0;
748     if (kx) {
749       for (int tx=0; tx < numOutcomes-1; tx++) {
750         int Tcell = tx * (numOutcomes-1) + kx-1;
751         ak[kx] += Ta[Tcell] * alpha[tx];
752         ck += Tc[Tcell] * gamma[tx];
753       }
754     }
755
756     double z = discr * ak[kx] + ck;
757     if (z < -EXP_STABLE_DOMAIN) z = -EXP_STABLE_DOMAIN;
758     else if (z > EXP_STABLE_DOMAIN) z = EXP_STABLE_DOMAIN;
759     num[kx] = z;
760   }
761 }
762
763
764 static void
765 irt_rpf_nominal_prob(const double *spec,
766                      const double *restrict param, const double *restrict th,
767                      double *restrict out)
768 {
769   int numOutcomes = spec[RPF_ISpecOutcomes];
770   int numDims = spec[RPF_ISpecDims];
771   double num[numOutcomes];
772   double ak[numOutcomes];
773   double discr = dotprod(param, th, numDims);
774   _nominal_rawprob(spec, param, th, discr, ak, num);
775   double den = 0;
776
777   for (int kx=0; kx < numOutcomes; kx++) {
778     num[kx] = exp(num[kx]);
779     den += num[kx];
780   }
781   for (int kx=0; kx < numOutcomes; kx++) {
782     out[kx] = num[kx]/den;
783   }
784 }
785
786 static void
787 irt_rpf_nominal_logprob(const double *spec,
788                         const double *restrict param, const double *restrict th,
789                         double *restrict out)
790 {
791   int numOutcomes = spec[RPF_ISpecOutcomes];
792   int numDims = spec[RPF_ISpecDims];
793   double num[numOutcomes];
794   double ak[numOutcomes];
795   double discr = dotprod(param, th, numDims);
796   _nominal_rawprob(spec, param, th, discr, ak, num);
797   double den = 0;
798
799   for (int kx=0; kx < numOutcomes; kx++) {
800     den += exp(num[kx]);
801   }
802   den = log(den);
803
804   for (int kx=0; kx < numOutcomes; kx++) {
805     out[kx] = num[kx] - den;
806   }
807 }
808
809 static double makeOffterm(const double *dat, const double p, const double aTheta,
810                           const int ncat, const int cat)
811 {
812   double ret = 0;
813   for (int CAT = 0; CAT < ncat; CAT++) {
814     if (CAT == cat) continue;
815     ret += dat[CAT] * p * aTheta;
816   }
817   return(ret);
818 }
819
820 static double makeOffterm2(const double *dat, const double p1, const double p2, 
821                            const double aTheta, const int ncat, const int cat)
822 {
823   double ret = 0;
824   for (int CAT = 0; CAT < ncat; CAT++) {
825     if (CAT == cat) continue;
826     ret += dat[CAT] * p1 * p2 * aTheta;
827   }
828   return(ret);
829 }
830
831 static void
832 irt_rpf_nominal_deriv1(const double *spec,
833                        const double *restrict param,
834                        const double *where, const double area,
835                        const double *weight, double *out)
836 {
837   int nfact = spec[RPF_ISpecDims];
838   int ncat = spec[RPF_ISpecOutcomes];
839   double aTheta = dotprod(param, where, nfact);
840   double aTheta2 = aTheta * aTheta;
841
842   double num[ncat];
843   double ak[ncat];
844   _nominal_rawprob(spec, param, where, aTheta, ak, num);
845
846   double P[ncat];
847   double P2[ncat];
848   double P3[ncat];
849   double ak2[ncat];
850   double dat_num[ncat];
851   double numsum = 0;
852   double numakD = 0;
853   double numak2D2 = 0;
854   double numakDTheta_numsum[nfact];
855
856   for (int kx=0; kx < ncat; kx++) {
857     num[kx] = exp(num[kx]);
858     ak2[kx] = ak[kx] * ak[kx];
859     dat_num[kx] = weight[kx]/num[kx];
860     numsum += num[kx];
861     numakD += num[kx] * ak[kx];
862     numak2D2 += num[kx] * ak2[kx];
863   }
864   double numsum2 = numsum * numsum;
865
866   for (int kx=0; kx < ncat; kx++) {
867     P[kx] = num[kx]/numsum;
868     P2[kx] = P[kx] * P[kx];
869     P3[kx] = P2[kx] * P[kx];
870   }
871
872   double sumNumak = dotprod(num, ak, ncat);
873   for (int fx=0; fx < nfact; fx++) {
874     numakDTheta_numsum[fx] = sumNumak * where[fx] / numsum;
875   }
876
877   for (int jx = 0; jx < nfact; jx++) {
878     double tmpvec = 0;
879     for(int i = 0; i < ncat; i++) {
880       tmpvec += dat_num[i] * (ak[i] * where[jx] * P[i] -
881                               P[i] * numakDTheta_numsum[jx]) * numsum;
882     }
883     out[jx] += area * tmpvec;
884   }
885   for(int i = 1; i < ncat; i++) {
886     double offterm = makeOffterm(weight, P[i], aTheta, ncat, i);
887     double offterm2 = makeOffterm(weight, P[i], 1, ncat, i);
888     double tmpvec = dat_num[i] * (aTheta * P[i] - P2[i] * aTheta) * numsum - offterm;
889     double tmpvec2 = dat_num[i] * (P[i] - P2[i]) * numsum - offterm2;
890     out[nfact + i - 1] += area * tmpvec;
891     out[nfact + ncat + i - 2] += area * tmpvec2;
892   }
893
894   int hessbase = nfact + (ncat-1)*2;
895   int d2ind = 0;
896   //a's
897   for (int j = 0; j < nfact; j++) {
898     for (int k = 0; k <= j; k++) {
899       double tmpvec = 0;
900       for (int i = 0; i < ncat; i++) {
901         tmpvec += dat_num[i] * (ak2[i] * where[j] * where[k] * P[i] -
902                                 ak[i] * where[j] * P[i] * numakDTheta_numsum[k] -
903                                 ak[i] * where[k] * P[i] * numakDTheta_numsum[j] + 
904                                 2 * P[i] * numakD * where[j] * numakD * where[k] / numsum2 -
905                                 P[i] * numak2D2 * where[j] * where[k] / numsum) * numsum - 
906           dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
907           numsum * ak[i] * where[k] +
908           dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
909           numakD * where[k];
910       }
911       out[hessbase + d2ind++] += area * tmpvec;
912     }
913   }
914   //a's with ak and d
915   for(int k = 1; k < ncat; k++){
916     int akrow = hessbase + (nfact+k)*(nfact+k-1)/2;
917     int dkrow = hessbase + (nfact+ncat+k-1)*(nfact+ncat+k-2)/2;
918     for(int j = 0; j < nfact; j++){
919       double tmpvec = 0;
920       double tmpvec2 = 0;
921       for(int i = 0; i < ncat; i++){
922         if(i == k){
923           tmpvec += dat_num[i] * (ak[i]*where[j] * aTheta*P[i] -
924                                      aTheta*P[i]*numakDTheta_numsum[j] +
925                                      where[j]*P[i] - 2*ak[i]*where[j]*aTheta*P2[i] +
926                                      2*aTheta*P2[i]*numakDTheta_numsum[j] -
927                                      where[j]*P2[i])*numsum -
928             dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*numsum*ak[i]*where[j] +
929             dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*(numakD*where[j]);
930           tmpvec2 += dat_num[i]*(ak[i]*where[j]*P[i] -
931                                       2*ak[i]*where[j]*P2[i] -
932                                       P[i]*numakDTheta_numsum[j] +
933                                       2*P2[i]*numakDTheta_numsum[j])*numsum -
934             dat_num[i]*(P[i] - P2[i])*numsum*ak[i]*where[j] +
935             dat_num[i]*(P[i] - P2[i])*(numakD*where[j]);
936         } else {
937           tmpvec += -weight[i]*ak[k]*aTheta*where[j]*P[k] +
938             weight[i]*P[k]*aTheta*numakDTheta_numsum[j] -
939             weight[i]*P[k]*where[j];
940           tmpvec2 += -weight[i]*ak[k]*where[j]*P[k] +
941             weight[i]*P[k]*numakDTheta_numsum[j];
942         }
943       }
944       out[akrow + j] += area * tmpvec;
945       out[dkrow + j] += area * tmpvec2;
946     }
947   }
948   //ak's and d's
949   for(int j = 1; j < ncat; j++){
950     int akrow = hessbase + (nfact+j)*(nfact+j-1)/2;
951     int dkrow = hessbase + (nfact+ncat+j-1)*(nfact+ncat+j-2)/2;
952
953     double tmpvec = makeOffterm(weight, P2[j], aTheta2, ncat, j);
954     double tmpvec2 = makeOffterm(weight, P[j], aTheta2, ncat, j);
955     double offterm = tmpvec - tmpvec2;
956     tmpvec = makeOffterm(weight, P2[j], 1, ncat, j);
957     tmpvec2 = makeOffterm(weight, P[j], 1, ncat, j);
958     double offterm2 = tmpvec - tmpvec2;
959
960     out[akrow + nfact + j - 1] +=
961       area * (dat_num[j]*(aTheta2*P[j] - 3*aTheta2*P2[j] +
962                           2*aTheta2*P3[j])*numsum - weight[j]/num[j] *
963               (aTheta*P[j] - aTheta*P2[j])*numsum*aTheta + weight[j] *
964               (aTheta*P[j] - aTheta*P2[j])*aTheta + offterm);
965
966     out[dkrow + nfact + ncat + j - 2] +=
967       area * (dat_num[j]*(P[j] - 3*P2[j] + 2*P3[j])*numsum - weight[j]/num[j] *
968               (P[j] - P2[j])*numsum + weight[j] *
969               (P[j] - P2[j]) + offterm2);
970
971     for(int i = 1; i < ncat; i++) {
972       if(j > i) {
973         offterm = makeOffterm2(weight, P[j], P[i], aTheta2, ncat, i);
974         offterm2 = makeOffterm2(weight, P[j], P[i], 1, ncat, i);
975         tmpvec = dat_num[i] * (-aTheta2*P[i]*P[j] + 2*P2[i] *aTheta2*P[j])*numsum + 
976           dat_num[i] * (aTheta*P[i] - P2[i] * aTheta)*aTheta*num[j]+offterm;
977         tmpvec2 = dat_num[i] * (-P[i]*P[j] + 2*P2[i] *P[j]) * numsum +
978           dat_num[i] * (P[i] - P2[i]) * num[j] + offterm2;
979         out[akrow + nfact + i - 1] += area * tmpvec;
980         out[dkrow + nfact + ncat + i - 2] += area * tmpvec2;
981       }
982       if (abs(j-i) == 0) {
983         tmpvec = makeOffterm(weight, P2[i], aTheta, ncat, i);
984         tmpvec2 = makeOffterm(weight, P[i], aTheta, ncat, i);
985         offterm = tmpvec - tmpvec2;
986         tmpvec = dat_num[i]*(aTheta*P[i] - 3*aTheta*P2[i] +
987                              2*aTheta*P3[i]) * numsum - dat_num[i] *
988           (aTheta*P[i] - aTheta*P2[i])*numsum + weight[i] *
989           (P[i] - P2[i])*aTheta + offterm;
990         out[dkrow + nfact + i - 1] += area * tmpvec;
991       } else {
992         offterm = makeOffterm2(weight, P[j], P[i], aTheta, ncat, i);
993         tmpvec = dat_num[i] * (-aTheta*P[i]*P[j] + 2*P2[i] *aTheta*P[j]) * numsum + 
994           dat_num[i] * (P[i] - P2[i]) * aTheta * num[j] + offterm;
995         out[dkrow + nfact + i - 1] += area * tmpvec;
996       }
997     }
998   }
999 }
1000
1001 static void
1002 pda(const double *ar, int rows, int cols) {   // column major order
1003         for (int rx=0; rx < rows; rx++) {
1004                 for (int cx=0; cx < cols; cx++) {
1005                         Rprintf("%.6g, ", ar[cx * rows + rx]);
1006                 }
1007                 Rprintf("\n");
1008         }
1009 }
1010
1011 static void
1012 irt_rpf_nominal_deriv2(const double *spec,
1013                        const double *restrict param,
1014                        double *out)
1015 {
1016   int nfact = spec[RPF_ISpecDims];
1017   int nzeta = spec[RPF_ISpecOutcomes] - 1;
1018   const double *aa = param;
1019
1020   for (int dx=0; dx < nfact; dx++) {
1021     if (aa[dx] < 0) {
1022       set_deriv_nan(spec, out);
1023       return;
1024     }
1025   }
1026
1027   const double *Ta = spec + RPF_ISpecCount;
1028   const double *Tc = spec + RPF_ISpecCount + nzeta * nzeta;
1029   const int numParam = irt_rpf_nominal_numParam(spec);
1030   double rawOut[numParam];
1031   memcpy(rawOut, out, sizeof(double) * numParam);
1032
1033   // gradient
1034   for (int tx=0; tx < nzeta; tx++) {
1035     double ak1=0;
1036     double ck1=0;
1037     for (int kx=0; kx < nzeta; kx++) {
1038       int Tcell = tx * nzeta + kx;
1039       ak1 += rawOut[nfact + kx] * Ta[Tcell];
1040       ck1 += rawOut[nfact + nzeta + kx] * Tc[Tcell];
1041     }
1042     out[nfact + tx] = ak1;
1043     out[nfact + nzeta + tx] = ck1;
1044   }
1045
1046   // don't need to transform the main a parameters TODO
1047   double *dmat = Realloc(NULL, 3 * numParam * numParam, double);
1048   const int hsize = hessianIndex(0, numParam-1, numParam-1);
1049   {
1050     int row=0;
1051     int col=0;
1052     for (int dx=0; dx <= hsize; dx++) {
1053       dmat[numParam * col + row] = out[numParam + dx];
1054       if (row == col) {
1055         col=0; ++row;
1056       } else {
1057         dmat[numParam * row + col] = out[numParam + dx];
1058         ++col;
1059       }
1060     }
1061   }
1062
1063   double *tmat = dmat + numParam * numParam;
1064   for (int dx=0; dx < numParam * numParam; dx++) tmat[dx] = 0;
1065   for (int dx=0; dx < nfact; dx++) {
1066     tmat[dx * numParam + dx] = 1;
1067   }
1068   for (int rx=0; rx < nzeta; rx++) {
1069     for (int cx=0; cx < nzeta; cx++) {
1070       tmat[(cx + nfact)*numParam + nfact + rx] = Ta[rx * nzeta + cx];
1071       tmat[(cx + nfact + nzeta)*numParam + nfact + nzeta + rx] = Tc[rx * nzeta + cx];
1072     }
1073   }
1074
1075   double *dest = dmat + 2 * numParam * numParam;
1076
1077   // It is probably possible to do this more efficiently than dgemm
1078   // since we know that we only care about the lower triangle.
1079   // I'm not sure whether this is worth optimizing. TODO
1080
1081   char normal = 'n';
1082   char transpose = 't';
1083   double one = 1;
1084   double zero = 0;
1085   F77_CALL(dgemm)(&normal, &normal, &numParam, &numParam, &numParam,
1086                   &one, tmat, &numParam, dmat, &numParam, &zero, dest, &numParam);
1087   F77_CALL(dgemm)(&normal, &transpose, &numParam, &numParam, &numParam,
1088                   &one, dest, &numParam, tmat, &numParam, &zero, dmat, &numParam);
1089
1090   {
1091     int row=0;
1092     int col=0;
1093     for (int dx=0; dx <= hsize; dx++) {
1094       out[numParam + dx] = dmat[numParam * col + row];
1095       if (row == col) {
1096         col=0; ++row;
1097       } else {
1098         ++col;
1099       }
1100     }
1101   }
1102
1103   Free(dmat);
1104 }
1105
1106 static void
1107 irt_rpf_mdim_nrm_dTheta(const double *spec, const double *param,
1108                         const double *where, const double *dir,
1109                         double *grad, double *hess)
1110 {
1111   int numDims = spec[RPF_ISpecDims];
1112   int outcomes = spec[RPF_ISpecOutcomes];
1113   const double *aa = param;
1114   double num[outcomes];
1115   double ak[outcomes];
1116   double discr = dotprod(param, where, numDims);
1117   _nominal_rawprob(spec, param, where, discr, ak, num);
1118
1119   double den = 0;
1120   for (int kx=0; kx < outcomes; kx++) {
1121     num[kx] = exp(num[kx]);
1122     den += num[kx];
1123   }
1124
1125   double P[outcomes];
1126   for (int kx=0; kx < outcomes; kx++) {
1127     P[kx] = num[kx]/den;
1128   }
1129
1130   for(int jx=0; jx < numDims; jx++) {
1131     double jak[outcomes];
1132     double jak2[outcomes];
1133     for (int ax=0; ax < outcomes; ax++) {
1134       jak[ax] = ak[ax] * aa[jx];
1135       jak2[ax] = jak[ax] * jak[ax];
1136     }
1137     double numjak = dotprod(num, jak, outcomes);
1138     double numjakden2 = numjak / den;
1139     numjakden2 *= numjakden2;
1140     double numjak2den = dotprod(num, jak2, outcomes) / den;
1141
1142     for(int ix=0; ix < outcomes; ix++) {
1143       grad[ix] += dir[jx] * (ak[ix] * aa[jx] * P[ix] - P[ix] * numjak / den);
1144       hess[ix] += dir[jx] * (ak[ix]*ak[ix] * aa[jx]*aa[jx] * P[ix] -
1145                              2 * ak[ix] * aa[jx] * P[ix] * numjak / den +
1146                              2 * P[ix] * numjakden2 - P[ix] * numjak2den);
1147     }
1148   }
1149 }
1150
1151 static void
1152 irt_rpf_mdim_nrm_rescale(const double *spec, double *restrict param, const int *paramMask,
1153                          const double *restrict mean, const double *restrict choleskyCov)
1154 {
1155   int numDims = spec[RPF_ISpecDims];
1156   int nzeta = spec[RPF_ISpecOutcomes] - 1;
1157   double *alpha = param + numDims;
1158   double *gamma = param + numDims + nzeta;
1159   const double *Ta  = spec + RPF_ISpecCount;
1160   const double *Tc  = spec + RPF_ISpecCount + nzeta * nzeta;
1161   const double *iTc = spec + RPF_ISpecCount + 3 * nzeta * nzeta;
1162
1163   for (int d1=0; d1 < numDims; d1++) {
1164     if (paramMask[d1] < 0) continue;
1165     param[d1] = dotprod(param+d1, choleskyCov + d1 * numDims + d1, numDims-d1);
1166   }
1167
1168   double madj = dotprod(param, mean, numDims);
1169
1170   double ak[nzeta];
1171   double ck[nzeta];
1172   memset(ak, 0, sizeof(double)*nzeta);
1173   memset(ck, 0, sizeof(double)*nzeta);
1174
1175   for (int kx=0; kx < nzeta; kx++) {
1176     for (int tx=0; tx < nzeta; tx++) {
1177       int Tcell = tx * nzeta + kx;
1178       ak[kx] += Ta[Tcell] * alpha[tx];
1179       ck[kx] += Tc[Tcell] * gamma[tx];
1180     }
1181   }
1182
1183   for (int kx=0; kx < nzeta; kx++) {
1184     ck[kx] += madj * ak[kx];
1185   }
1186
1187   for (int kx=0; kx < nzeta; kx++) {
1188     int px = numDims + nzeta + kx;
1189     if (paramMask[px] < 0) continue;
1190
1191     param[px] = 0;
1192
1193     for (int tx=0; tx < nzeta; tx++) {
1194       int Tcell = tx * nzeta + kx;
1195       param[px] += iTc[Tcell] * ck[tx];
1196     }
1197   }
1198 }
1199
1200 static void noop() {}
1201 static void notimplemented() { error("Not implemented"); }
1202
1203 const struct rpf librpf_model[] = {
1204   { "drm1-",
1205     irt_rpf_1dim_drm_numSpec,
1206     irt_rpf_1dim_drm_numParam,
1207     irt_rpf_mdim_drm_paramBound,
1208     irt_rpf_1dim_drm_prob,
1209     irt_rpf_logprob_adapter,
1210     irt_rpf_1dim_drm_deriv1,
1211     irt_rpf_1dim_drm_deriv2,
1212     notimplemented,
1213     irt_rpf_1dim_drm_rescale,
1214   },
1215   { "drm1",
1216     irt_rpf_1dim_drm_numSpec,
1217     irt_rpf_1dim_drm_numParam,
1218     irt_rpf_mdim_drm_paramBound,
1219     irt_rpf_1dim_drm_prob,
1220     irt_rpf_logprob_adapter,
1221     notimplemented,
1222     notimplemented,
1223     irt_rpf_1dim_drm_dTheta,
1224     notimplemented,
1225   },
1226   { "drm1+",
1227     irt_rpf_mdim_drm_numSpec,
1228     irt_rpf_mdim_drm_numParam,
1229     irt_rpf_mdim_drm_paramBound,
1230     irt_rpf_mdim_drm_prob,
1231     irt_rpf_logprob_adapter,
1232     irt_rpf_mdim_drm_deriv1,
1233     irt_rpf_mdim_drm_deriv2,
1234     notimplemented,
1235     irt_rpf_mdim_drm_rescale,
1236   },
1237   { "drm",
1238     irt_rpf_mdim_drm_numSpec,
1239     irt_rpf_mdim_drm_numParam,
1240     irt_rpf_mdim_drm_paramBound,
1241     irt_rpf_mdim_drm_prob,
1242     irt_rpf_logprob_adapter,
1243     irt_rpf_mdim_drm_deriv1,
1244     irt_rpf_mdim_drm_deriv2,
1245     irt_rpf_mdim_drm_dTheta,
1246     irt_rpf_mdim_drm_rescale,
1247   },
1248   { "grm1",
1249     irt_rpf_mdim_grm_numSpec,
1250     irt_rpf_mdim_grm_numParam,
1251     irt_rpf_mdim_grm_paramBound,
1252     irt_rpf_1dim_grm_prob,
1253     irt_rpf_logprob_adapter,
1254     noop, // TODO fill in pre/post transformation
1255     noop,
1256     notimplemented,
1257     noop,
1258   },
1259   { "grm",
1260     irt_rpf_mdim_grm_numSpec,
1261     irt_rpf_mdim_grm_numParam,
1262     irt_rpf_mdim_grm_paramBound,
1263     irt_rpf_mdim_grm_prob,
1264     irt_rpf_logprob_adapter,
1265     irt_rpf_mdim_grm_deriv1,
1266     irt_rpf_mdim_grm_deriv2,
1267     irt_rpf_mdim_grm_dTheta,
1268     irt_rpf_mdim_grm_rescale,
1269   },
1270   { "nominal",
1271     irt_rpf_nominal_numSpec,
1272     irt_rpf_nominal_numParam,
1273     irt_rpf_nominal_paramBound,
1274     irt_rpf_nominal_prob,
1275     irt_rpf_nominal_logprob,
1276     irt_rpf_nominal_deriv1,
1277     irt_rpf_nominal_deriv2,
1278     irt_rpf_mdim_nrm_dTheta,
1279     irt_rpf_mdim_nrm_rescale,
1280   }
1281 };
1282
1283 const int librpf_numModels = (sizeof(librpf_model) / sizeof(struct rpf));