## C++ version of Joe Lang's software mph.fit
## to provide restricted MLEs

#.libPaths("C:/R/library")
require(inline)
require(RcppArmadillo)

## Set up all variables at first;
src <- '

arma::mat y = Rcpp::as<arma::mat>(yr);
arma::mat Z = Rcpp::as<arma::mat>(Zr);
arma::mat C = Rcpp::as<arma::mat>(Cr);
arma::mat delta = Rcpp::as<arma::mat>(deltar);
double yeps = as<double>(epsr);
int lenm = y.n_rows;
//double delta_low=REAL(deltamin)[0];
//double delta_high=REAL(deltamax)[0];
arma::mat y_orig = y;
int is_original_y = 1;
y = y_orig + yeps;
if (yeps != 0) {
    is_original_y = 0;
}

int maxiter = 100;
double step = 1;
double change_step_after = 0.25*maxiter;
double iter_orig = 10;
double norm_diff_conv = 0.0000001;
double norm_score_conv = 0.0000001;
int max_score_diff_iter = 10;


arma::mat ZF = Z;
arma::mat ZZt = Z * arma::trans(Z);
arma::mat m = y_orig + yeps;
int j=0;
for(j = 0; j < (int)m.n_rows; j++) {
   if(m(j)==0) m(j)=0.01;
}
arma::mat xi = arma::log(m);
arma::mat p =  m / (ZZt * m);
arma::mat h = C*p - delta;
//arma::mat h = arma::log((C*p-delta_low)/(delta_high-C*p)) - delta;
//arma::mat hobs = h;

double lenh = h.n_rows;
arma::mat lam = arma::zeros(lenh, 1);
arma::mat Dm = arma::diagmat(m);
arma::mat Dminv = arma::diagmat(1/m);

// DERIVATIVE
double eps = 0.000006055454; //based on R
arma::mat d = eps*m+eps;
arma::mat E = arma::diagmat(d);

arma::mat H(delta.n_rows, d.n_rows);

for(j = 0; j < (int)d.n_rows; j++){
    H.col(j) = ((C*( (m+E.col(j)) /  (ZZt * (m+E.col(j))) ) - delta)-(C*( (m-E.col(j)) /  (ZZt * (m-E.col(j))) ) - delta)) / (2*arma::as_scalar(d.row(j)));
}
H = arma::trans(H);

//DERIVATIVE ENDS

arma::mat HtDHinv = arma::inv(arma::trans(H) * (H % arma::repmat(m,1,delta.n_rows)));
arma::mat HtDHinvobs = HtDHinv;
arma::mat HHtDHinv = H * HtDHinv;
arma::mat s = arma::join_cols(y-m+(H % arma::repmat(m,1,delta.n_rows))*lam, h);

//double norm_score = sqrt(arma::accu(s*s)); THIS MIGHT NOT BE RIGHT.
double norm_score = arma::as_scalar(arma::sqrt(arma::trans(s)*s));

arma::mat theta = arma::join_cols(xi, lam);


int lentheta = theta.n_rows;
int iter = 0;
int score_diff_iter = 0;
double norm_diff = 10;
double step_start = step;
int number_increase = 0;
arma::mat theta_temp;
arma::mat s_temp;
arma::mat norm_score_temp;


while (((norm_diff > norm_diff_conv) ||
	(norm_score > norm_score_conv) ||
	(!is_original_y)) &&
	(iter< maxiter) &&
	(score_diff_iter < max_score_diff_iter)){

    if (iter == iter_orig) {
    y = y_orig;
		is_original_y = 1;
	}
	arma::mat A = arma::join_rows(Dminv - HHtDHinv * arma::trans(H), -HHtDHinv);
       	A = arma::join_cols(A, arma::join_rows(-arma::trans(HHtDHinv),-HtDHinv));

//     ####################### FINE-TUNE UPDATING STEP #######################
       double step_temp = step_start;
       int step_iter = 0;
       int step_out = 0;



//     ##############  BEGIN while ###################################
      while ((step_iter < 5) && (step_out == 0)) {
         theta_temp = theta + step_temp*(A*s);
         norm_diff = arma::as_scalar(arma::sqrt(arma::trans(theta-theta_temp)*(theta-theta_temp)));
         m = arma::exp(theta_temp.rows(0,lenm-1));
         lam = theta_temp.rows(lenm, lentheta-1);
	       p = m % (1/(Z * arma::trans(Z) * m));
         h = C*p - delta;
         //h = arma::log((C*p-delta_low)/(delta_high-C*p)) - delta;;

        Dm = arma::diagmat(m);
        Dminv = arma::diagmat(1/m);

        d = eps*m+eps;
        E = arma::diagmat(d);
        H.set_size(delta.n_rows, d.n_rows);

        for(j = 0; j < (int)d.n_rows; j++){
            H.col(j) = ((C*( (m+E.col(j)) /  (ZZt * (m+E.col(j))) ) - delta)-(C*( (m-E.col(j)) /  (ZZt * (m-E.col(j))) ) - delta)) / (2*arma::as_scalar(d.row(j)));
        }
	      H = arma::trans(H);

        HtDHinv = arma::inv(arma::trans(H) * (H % arma::repmat(m,1,delta.n_rows)));
        HHtDHinv = H * HtDHinv;
        s_temp = arma::join_cols(y-m+((H % arma::repmat(m,1,delta.n_rows))*lam), h);
        norm_score_temp = arma::sqrt(arma::trans(s_temp)*s_temp);

        if (!(norm_score_temp.is_finite())) {
          step_iter++;
		      if(step_temp/2 > 0.01){
			       step_temp = step_temp/2;
		      } else {
          step_temp = 0.01;
		      }
        } else {
        if (arma::as_scalar(norm_score_temp) >  norm_score) {
          number_increase++;
         	if (arma::as_scalar(norm_score_temp) > (2*norm_score)
		        && arma::as_scalar(norm_score_temp) > norm_score_conv) {
		        step_iter++;
            // maximum fct begin
            if(step_temp/2 > 0.01){
				        step_temp = step_temp/2;
            } else {
				        step_temp = 0.01;
            }
            // maximum fct ends
          } else {
          step_out = 1;
		      }
        } else {
		    step_out = 1;
        }
      }
    }
//     #################  END while ((step.iter < 5)&(step.out==F)) ####

    if (number_increase > change_step_after) {
		  if(step_start/2 > 0.01){
			 step_start = step_start/2;
		  } else {
			step_start = 0.01;
		  }
    number_increase = 0;
    }

//       ####################  END FINE-TUNE UPDATING STEP #######################

       theta = theta_temp;
       s = s_temp;
       norm_score = arma::as_scalar(norm_score_temp);
       iter++;
       if ((norm_score < norm_score_conv) && (norm_diff > norm_diff_conv)) {
              score_diff_iter++;
       }
   }

  // ########################  END ITERATING ##################################
 	p = m % (1/(Z * arma::trans(Z) * y));
	//return Rcpp::List::create(Rcpp::Named("fit") = m);
  return Rcpp::wrap(p);
'
mph.fast <- cxxfunction(signature(yr="numeric", Zr="numeric", Cr = "numeric", deltar = "numeric", epsr="numeric"), src, plugin="RcppArmadillo", verbose = FALSE)