#include <Rcpp.h>
using namespace Rcpp;

// [[Rcpp::export]]
double matrixloglikC(NumericMatrix data, NumericMatrix means, List covmat, NumericMatrix b, NumericMatrix c) {
    double L=0;
    double z[2];
    double z_vector[100]={-2.5758,   -2.1900,   -1.9673,   -1.8157,   -1.6978,   -1.5998,   -1.5153,   -1.4404,   -1.3729 ,  -1.3112 ,  -1.2540 ,  -1.2008,   -1.1507,
                   -1.1034,   -1.0584,   -1.0154,   -0.9743,   -0.9348 ,  -0.8966,   -0.8598 ,  -0.8240,   -0.7893 ,  -0.7555  , -0.7226 ,  -0.6904,   -0.6589,
                   -0.6281,   -0.5978,   -0.5681,   -0.5389,   -0.5101 ,  -0.4816,   -0.4538 ,  -0.4262,   -0.3989 ,  -0.3719,   -0.3452 ,  -0.3167,   -0.2924,
                   -0.2663,   -0.2404,   -0.2147,   -0.1691,   -0.1637 ,  -0.1383,   -0.1130 ,  -0.0879,   -0.0627 ,  -0.0376,   -0.0125 ,   0.0125,    0.0376,
                   0.0627,    0.0879,   0.1130 ,   0.1383  ,  0.1637 ,   0.1691  ,  0.2147   , 0.2404  ,  0.2663   , 0.2924  ,  0.3167   , 0.3452  ,  0.3719,
                   0.3989,    0.4262,    0.4538,    0.4816 ,   0.5101,    0.5389 ,   0.5681  ,  0.5978 ,   0.6281  ,  0.6589 ,   0.6904  ,  0.7226 ,   0.7555,
                   0.7893,    0.8240,    0.8598,    0.8966 ,   0.9348,    0.9743 ,   1.0154  ,  1.0584 ,   1.1034  ,  1.1507 ,   1.2008  ,  1.2540 ,   1.3112,
                   1.3729,    1.4404,    1.5153,    1.5998 ,   1.6978,    1.8157 ,   1.9673  ,  2.1900 ,   2.5758};

    for (int i=0; i<4; i++){
        NumericMatrix P(2,2);
        NumericMatrix q=covmat[i];
        P(0,0)=sqrt(q(0,0)); P(0,1)=0; P(1,0)=q(1,0)/(P(0,0)); P(1,1)=sqrt(q(1,1)-pow(P(1,0),2));
        
        
        NumericMatrix scal(2,2);
        for (int w=0;w<2;w++){
            for(int e=0;e<2;e++){
                scal(w,e)=0;
                for(int r=0;r<2;r++){
                    scal(w,e)=scal(w,e)+b(w,r)*P(r,e);
        }}}
        
        double cons[2];
        for(int s=0;s<2;s++){
		cons[s]=c(s,0);
		for(int d=0;d<2;d++){
    			cons[s]=cons[s]+b(s,d)*means(i,d);
        }}

        double p[4]={.000001,.000001,.000001,.000001};
        for(int a=0;a<100;a++){
		z[0]=z_vector[a];
		for(int f=0;f<100;f++){
			z[1]=z_vector[f];
			
			double h[2];
			for(int g=0;g<2;g++){
				h[g]=cons[g];
				for(int y=0;y<2;y++){
		    			h[g]=h[g]+scal(g,y)*z[y];
			}}
			
			if(h[0]<0){
				if(h[1]<0){
					p[0]=p[0]+0.0001;
				}else{
					p[2]=p[2]+0.0001;}
			}else{
				if (h[1]<0){
					p[1]=p[1]+0.0001;
				}else{
					p[3]=p[3]+0.0001;
			}} 
		}}
        
        for(int y=0;y<4;y++){ 
        	L+=log(p[y]/1.000004)*data(i,y);
            }
    }
    return L;
}

