#include "libscl.h"
using namespace scl;
using namespace std;

int main(int argc, char** argp, char** envp)
{

  string program(argp[0]);
  
  if (argc != 5) {
    cerr << '\n';
    cerr << "Usage:   " << program << " prefix start stop l_cmf_parms" << '\n';
    cerr << "Example: " << program << " sdf.parms_chain 0 9 3" << '\n';
    cerr << "Comment: sdf.parms_chain.000.dat would be first file read"<<'\n';
    cerr << "         sdf.parms_chain.009.dat would be last for exmple."<<'\n';
    cerr << '\n';
    return 1;
  }

  string rootname(argp[1]);
  INTEGER start(atoi(argp[2]));
  INTEGER stop(atoi(argp[3]));
  INTEGER l_cmf_parms(atoi(argp[4]));

  cout << '\n';
  cout << program << " settings;" << '\n';
  cout << "  rootname = " << rootname << '\n';
  cout << "  start = " << start << '\n';
  cout << "  stop = " << stop << '\n';
  cout << "  l_cmf_parms    = " << l_cmf_parms << '\n';
  cout << '\n';

  string::iterator sitr = rootname.end();

  if (*sitr != '.') rootname.push_back('.');

  string filename = rootname + fmt('d',3,start)('0') + ".dat";
  realmat parms_k;
  vecread(filename.c_str(),parms_k); 
  cout << "Read " << filename << '\n';

  INTEGER rows = parms_k.nrow();
  INTEGER cols = parms_k.ncol();

  INTEGER K = stop - start + 1;
  INTEGER max_cols = K*cols;

  realmat parms(rows,max_cols);

  INTEGER loc = 0;

  for (INTEGER j=1; j<=cols; ++j) {
    if (j > max_cols) break;
    for (INTEGER i=1; i<=rows; ++i) {
      parms(i,loc+j) = parms_k(i,j);
    }
  }

  loc += cols;

  for (INTEGER k=start+1; k<=stop; ++k) {
    string filename = rootname + fmt('d',3,k)('0') + ".dat";
    vecread(filename.c_str(),parms_k); 
    cout << "Read " << filename << '\n';

    if (rows != parms_k.nrow()) error("Error, number of rows differ");

    cols = parms_k.ncol();
    
    for (INTEGER j=1; j<=cols; ++j) {
      if (j > max_cols) break;
      for (INTEGER i=1; i<=rows; ++i) {
        parms(i,loc+j) = parms_k(i,j);
      }
    }

    loc += cols;
  }

  cout << '\n';
  
  INTEGER da = rows;
  INTEGER n = parms.ncol();
  INTEGER l_snp_parms = da - l_cmf_parms;
  INTEGER M = l_cmf_parms;
  INTEGER p = 1 + 2*l_snp_parms;

  cout << "da = " << da << '\n';
  cout << "l_snp_parms = " << l_snp_parms << '\n';
  cout << "l_cmf_parms = " << l_cmf_parms << '\n';
  cout << "n  = " << n << '\n';
  cout << "M  = " << M << '\n';
  cout << "p  = " << p << '\n';
  cout << '\n';

  // y = Bx + e
  // yx' = Bxx' + e'
  // B = (sum yx')inv(sum xx')
  
  realmat y(M,1);
  realmat x(p,1);

  realmat yy(M,M,0.0);
  realmat yx(M,p,0.0);
  realmat xx(p,p,0.0);
  realmat ybar(M,1,0.0);

  realmat B(M,p);
  realmat ee(M,M);
  realmat V(M,M);
  
  for (INTEGER j=1; j<=n; ++j) {
    x[1] = 1.0;
    for (INTEGER i=1; i<=l_snp_parms; ++i) {
      x[1+i] = parms(i,j);
      x[1+l_snp_parms+i] = pow(x[1+i],2);
    }
    for (INTEGER i=1; i<=M; ++i) {
       y[i] = parms(l_snp_parms+i,j);
    }
    yy += y*T(y);
    yx += y*T(x);
    xx += x*T(x);
    ybar += y;
  }

  realmat xxinv = invpsd(xx);
  B = yx*xxinv;
  ee = yy - yx*T(B);
  V = ee/n;
  ybar = ybar/n;

  cout << '\n';
  cout << "B" << B << '\n';
  cout << "V" << V << '\n';

  realmat sdev(M,1);
  for (INTEGER i=1; i<=M; ++i) sdev[i] = sqrt(V(i,i));

  cout << "ybar,sdev" << cbind(ybar,sdev) << '\n';
  
  ofstream fout;

  filename = "emitted_code.h";
  fout.open(filename.c_str());
  if (!fout) error("Error, cannot open " + filename);

  fout << "#ifndef __FILE_EMITTED_CODE_H_SEEN__" << '\n'; 
  fout << "#define __FILE_EMITTED_CODE_H_SEEN__" << '\n'; 
  fout << '\n';
  fout << "#include \"libscl.h\" " << '\n';
  fout << '\n';
  fout << "extern scl::realmat theta(const scl::realmat& rho, "; 
  fout << "INTEGER l_cmf_parms);" << '\n';
  fout << '\n';
  fout << "#endif" << '\n';
  fout.clear(); fout.close();
   
  filename = "emitted_code.cpp";
  fout.open(filename.c_str());
  if (!fout) error("Error, cannot open " + filename);

  fout << "#include \"emitted_code.h\"" << '\n';
  fout << '\n';
  fout << "using namespace scl;" << '\n';
  fout << "using namespace std;" << '\n';
  fout << '\n';
  fout << "realmat theta(const realmat& rho, INTEGER l_cmf_parms)" << '\n';  
  fout << '{' << '\n';
  fout << "  INTEGER l_snp_parms = rho.size();" << '\n';
  fout << "  if (l_snp_parms != " << l_snp_parms << ") {" << '\n';
  fout << "    error(\"Error, rho wrong size\");" << '\n';
  fout << "  };" << '\n';
  fout << "  INTEGER M = l_cmf_parms;" << '\n';
  fout << "  INTEGER p = 1 + 2*l_snp_parms;" << '\n';
  fout << "  realmat x(p,1);" << '\n';
  fout << "  x[1] = 1.0;" << '\n';
  fout << "  for (INTEGER i=1; i<=l_snp_parms; ++i) {" << '\n';
  fout << "    x[1+i] = rho[i];" << '\n';
  fout << "    x[1+l_snp_parms+i] = pow(x[1+i],2);" << '\n';
  fout << "  };" << '\n';
  fout << "  realmat B(M,p);" << '\n';
  for (INTEGER j=1; j<=p; ++j) {
    for (INTEGER i=1; i<=M; ++i) {
      fout << "  B(" << i << ',' << j << ") = ";
      fout << fmt('e',24,16,B(i,j)) << ';' << '\n';
    }
  }  
  fout << "  return B*x;" << '\n';
  fout << '}' << '\n';

  fout.clear(); fout.close();

  return 0;
}
