#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <rpyfmm.h>
#include <mkl.h>

//bradius: bead size 
//gap: distance between two rigid body;
//spacing: distance between two beads;
int main(){
    int N, nc = 4,i,j;
    double bradius = 0.1; //1.0/(2*sqrt(2)),
    double gap = 4.0, spacing=0.0; 
	
	double beta = 0.0;
    int accuracy = 6, s = 80;

    double *ShellSphs, ShellRadius; 
    ShellSphs = (double*)calloc(pow(nc,3)+3*nc*pow(nc-1,2), sizeof(double));
  /*===============================================================================
      Step 1 : Construct the Shell representation
   ==============================================================================*/
    //Construct sphere 1
    N = shell_model(nc, bradius,spacing, &ShellRadius, ShellSphs);
    double length = 2*ShellRadius;
    
	printf("Sphs1, N=%d,ShellRadisu = %f\n",N,ShellRadius);    
	for (i=0;i<N; i++)
	 printf("%.2f, %.2f, %.2f\n", ShellSphs[3*i], ShellSphs[3*i+1], ShellSphs[3*i+2]);

    // Construct Sphere 2 from sphere 1
    double offset = (2*ShellRadius + gap); // Distance between two sphere
    printf("offset = %f\n", offset); 
    for(i=0; i< N; i++){
		ShellSphs[3*(N+i)] = ShellSphs[3*i];
		ShellSphs[3*(N+i)+1] = ShellSphs[3*i+1];
    	ShellSphs[3*(N+i)+2] = ShellSphs[3*i+2]+ offset;
    }
	
    printf("Sphs2 is \n");
    for(i=N;i<2*N; i++)
	printf("%.2f, %.2f, %.2f\n", ShellSphs[3*i], ShellSphs[3*i+1], ShellSphs[3*i+2]);

    // Matrix computation
    double *T, *Q, *b, *y;
    T = (double*)calloc(3*N*6, sizeof(double)); //3N*6
    Q = (double*)calloc(6*3*N, sizeof(double));// 6*3N
    b = (double*)calloc(6*N, sizeof(double)); // b = T*F
    
    //Compute T and Q matrix
    TandQ(T, Q, ShellSphs, N);  
 
  /*================================================================================
      Step 2 : Given the external Force, compute the force on beads 
   			D*S = T*F = b
                     
                        => x = T'*S = (T'* D^{-1} * T )*F = A*F 
   Note : Here, since the two sphere are with the same no. of beads,T1 = T2 = T.
   So we basically use the same T but with different right hand side F to caculate vb
   or body 1 and body 2.
   ================================================================================*/
    double F[12]={0.0, 0.0, 1.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0};
    //cblas_dgemv (const CBLAS_LAYOUT Layout, const CBLAS_TRANSPOSE trans, const MKL_INT m, const MKL_INT n, const double alpha, const double *a, const MKL_INT lda, const double *x, const MKL_INT incx, const double beta, double *y, const MKL_INT incy);
    
    //1. Compute b = T*F
    MKL_INT m = 3*N;
    double *f;
    f = (double *)calloc(6, sizeof(double));
    double alpha = 1.0;
    for(i=0; i<6; i++) f[i]=F[i];
    cblas_dgemv(CblasRowMajor, CblasNoTrans, m, 6, alpha, T, 6, f, 1, 0, b,1);
	
    printf("b = T*F is as below \n");
    for(i =0; i<2*N;i++) 
	   printf("%.2f %.2f %.2f \n", b[3*i], b[3*i+1],b[3*i+2] );
    
    //2. Solve D*y = b using Krylov subspace method
        MKL_INT nparts = 2*N, dim = 6*N;
	printf("dim = %d, nparts = %d \n", dim, nparts);
        y = (double *)calloc(dim,sizeof(double));
	KrylovFMM(length, beta, s, accuracy, nparts, ShellSphs, b, y);
   /*=============================================================================
     Step 3 : Use iterative method to compute V = Z^(-1)*y
    =============================================================================*/
    //ComputeBodyVelocity();
    
    free(ShellSphs);
    free(T);
    free(Q);
    free(y);
    free(b);
 //   free(b2);
    free(f);
    return 0;
}