/*=================================================================
 *
 * YPRIME.C	Sample .MEX file corresponding to YPRIME.M
 *	        Solves simple 3 body orbit problem 
 *
 * The calling syntax is:
 *
 *		[yp] = yprime(t, y)
 *
 *  You may also want to look at the corresponding M-code, yprime.m.
 *
 * This is a MEX-file for MATLAB.  
 * Copyright 1984-2006 The MathWorks, Inc.
 *
 *=================================================================*/
/* $Revision: 1.10.6.4 $ */
#include <math.h>
#include "mex.h"


static void CALC_GRAD_C_N_STEPS( 
        double Energy[],
        double Gax[],
        double Gay[],
        double Gbx[],
        double Gby[],
        double Ax[], 
        double Ay[],
        double Bx[],
        double By[],
        double Ka_lin[],
        double Ka_con[],
        double Ao_con[],
        double Ka[],
        double Ao[],
        double Kl[],
        double Lo[],
        double Kb[],
        double Kb_lin[],
        double Bo[],
        double Kv[],
        double Vo[],
        double Ky[],
        double Yo[],
        double R_out[],
        double R_in[],
        double beta[],
        double n_steps[],
        double Is_done[])
{   
    int i;double step = 0;
    double tmp_dax, tmp_day, tmp_a;
    double tmp_dbx, tmp_dby, tmp_b;
    double tmp_dlx, tmp_dly, tmp_l;
    
    double tmp, Y_tmp = 0, R_tmp, f_tmp;
    double Vtmp[80];
    double m_a_f = 0.06;

    double E_o = 5000*Kv[0];/*  for the membrane force */
    double K = 500;
    
    double Energy_old = Energy[0];
    Is_done[0] = 1;
    
    while (step < n_steps[0]){
        step = step + 1.0;
        for (i=0;i<80;i++){
            Gax[i] = 0;Gay[i] = 0;
            Gbx[i] = 0;Gby[i] = 0;
            Vtmp[i] = 0;
            Energy[0] = 0;
            Y_tmp = 0;
        }
        
        for (i=0;i<80;i++){
            /* Apical surface */
            tmp_dax = Ax[(i+1)%80] - Ax[i];
            tmp_day = Ay[(i+1)%80] - Ay[i];
            tmp_a = sqrt(tmp_dax*tmp_dax + tmp_day*tmp_day);
            
            /*   Decrease the constriction strength when area is before m_a_f  */
            if (tmp_a < m_a_f*Ao[i]){
                f_tmp = Ka_lin[i]*tmp_a/(m_a_f*Ao[i]);
                Energy[0] += Ka_lin[i]*tmp_a*tmp_a/(m_a_f*Ao[i]);
            }
            else{
                f_tmp = Ka_lin[i];
                Energy[0] += Ka_lin[i]*tmp_a;
            }
            /* */
            
            tmp = (2.0*Ka[i]*(tmp_a - Ao[i]) + 2.0*Ka_con[i]*(tmp_a - Ao_con[i]) + f_tmp)/tmp_a;
            Energy[0] += Ka[i]*(tmp_a - Ao[i])*(tmp_a - Ao[i]) + Ka_con[i]*(tmp_a - Ao_con[i])*(tmp_a - Ao_con[i]);

            Gax[(i+1)%80] -= tmp*tmp_dax; Gay[(i+1)%80] -= tmp*tmp_day;
            Gax[i] += tmp*tmp_dax; Gay[i] += tmp*tmp_day;
            
            
            /* Basal surface  */
            tmp_dbx = Bx[(i+1)%80] - Bx[i];
            tmp_dby = By[(i+1)%80] - By[i];
            tmp_b = sqrt(tmp_dbx*tmp_dbx + tmp_dby*tmp_dby);
            tmp = (2.0*Kb[i]*(tmp_b - Bo[i])/tmp_b + Kb_lin[i])/tmp_b;
            
            Gbx[(i+1)%80] -= tmp*tmp_dbx; Gby[(i+1)%80] -= tmp*tmp_dby;
            Gbx[i] += tmp*tmp_dbx; Gby[i] += tmp*tmp_dby;
            Energy[0] += Kb[i]*(tmp_b - Bo[i])*(tmp_b - Bo[i]) + Kb_lin[i]*tmp_b;
        
            /* Lateral surface  */
            tmp_dlx = Bx[(i+1)%80] - Ax[(i+1)%80];
            tmp_dly = By[(i+1)%80] - Ay[(i+1)%80];
            tmp_l = sqrt(tmp_dlx*tmp_dlx + tmp_dly*tmp_dly);
            tmp = 2.0*Kl[(i+1)%80]*(tmp_l - Lo[(i+1)%80])/tmp_l;
            
            Gax[(i+1)%80] += tmp*tmp_dlx; Gay[(i+1)%80] += tmp*tmp_dly;
            Gbx[(i+1)%80] -= tmp*tmp_dlx; Gby[(i+1)%80] -= tmp*tmp_dly;
            Energy[0] += Kl[(i+1)%80]*(tmp_l - Lo[(i+1)%80])*(tmp_l - Lo[(i+1)%80]);
            
            /* Sum up the volume */
            tmp = (Ax[(i+1)%80] + Bx[(i+1)%80])*tmp_dly/4.0 - (Ay[(i+1)%80] + By[(i+1)%80])*tmp_dlx/4.0;
            Vtmp[(i+1)%80] -= tmp;
            Vtmp[i] += tmp;
            Vtmp[i] += (Ax[(i+1)%80] + Ax[i])*tmp_day/4.0 - (Ay[(i+1)%80] + Ay[i])*tmp_dax/4.0;
            Vtmp[i] -= (Bx[(i+1)%80] + Bx[i])*tmp_dby/4.0 - (By[(i+1)%80] + By[i])*tmp_dbx/4.0;
            
            /* Sum up the yolk  */
            Y_tmp += (Bx[(i+1)%80] + Bx[i])*tmp_dby/4.0 - (By[(i+1)%80] + By[i])*tmp_dbx/4.0;

    	   if (i>15 && i<35){
	       	tmp = sqrt((Bx[i-1] - Ax[i-1])*(Bx[i-1] - Ax[i-1]) + (By[i-1] - Ay[i-1])*(By[i-1] - Ay[i-1]));
			R_tmp = (-(By[i-1] - Ay[i-1])*(Bx[i] - Ax[i-1]) + (By[i] - Ay[i-1])*(Bx[i-1] - Ax[i-1]))/tmp;
			Energy[0] += E_o*1/(1 + exp(K*R_tmp));

			Gbx[i] += E_o*K*exp(K*R_tmp)/(1+exp(K*R_tmp))/(1+exp(K*R_tmp))*(By[i-1] - Ay[i-1])/tmp;
			Gby[i] += -E_o*K*exp(K*R_tmp)/(1+exp(K*R_tmp))/(1+exp(K*R_tmp))*(Bx[i-1] - Ax[i-1])/tmp;
		
      		}
        
    	    if (i>45 && i<65){
       	    tmp = sqrt((Bx[i+1] - Ax[i+1])*(Bx[i+1] - Ax[i+1]) + (By[i+1] - Ay[i+1])*(By[i+1] - Ay[i+1]));
			R_tmp = (-(By[i+1] - Ay[i+1])*(Bx[i] - Ax[i+1]) + (By[i] - Ay[i+1])*(Bx[i+1] - Ax[i+1]))/tmp;
			Energy[0] += E_o*1/(1 + exp(K*R_tmp));

			Gbx[i] += -E_o*K*exp(K*R_tmp)/(1+exp(K*R_tmp))/(1+exp(K*R_tmp))*(By[i+1] - Ay[i+1])/tmp;
			Gby[i] += E_o*K*exp(K*R_tmp)/(1+exp(K*R_tmp))/(1+exp(K*R_tmp))*(Bx[i+1] - Ax[i+1])/tmp;
        	}
        }
        
        /* Gradient and Energy for the volume and the Yolk  */
        Energy[0] += Ky[0]*(Y_tmp - Yo[0])*(Y_tmp - Yo[0]);
        Energy[0] += Ky[1]*Yo[0];
        for (i=0;i<80;i++){
            Gax[(i+1)%80] -= Kv[i]*(Vtmp[i] - Vo[i])*(By[(i+1)%80] - Ay[i])/2.0 + Kv[(i+1)%80]*(Vtmp[(i+1)%80] - Vo[(i+1)%80])*(Ay[(i+2)%80] - By[(i+1)%80])/2.0;
            Gay[(i+1)%80] -= Kv[i]*(Vtmp[i] - Vo[i])*(Ax[i] - Bx[(i+1)%80])/2.0 + Kv[(i+1)%80]*(Vtmp[(i+1)%80] - Vo[(i+1)%80])*(Bx[(i+1)%80] - Ax[(i+2)%80])/2.0;
            
            Gbx[(i+1)%80] -= Kv[i]*(Vtmp[i] - Vo[i])*(By[i] - Ay[(i+1)%80])/2.0 + Kv[(i+1)%80]*(Vtmp[(i+1)%80] - Vo[(i+1)%80])*(Ay[(i+1)%80] - By[(i+2)%80])/2.0;
            Gby[(i+1)%80] -= Kv[i]*(Vtmp[i] - Vo[i])*(Ax[(i+1)%80] - Bx[i])/2.0 + Kv[(i+1)%80]*(Vtmp[(i+1)%80] - Vo[(i+1)%80])*(Bx[(i+2)%80] - Ax[(i+1)%80])/2.0;
            Energy[0] += Kv[i]*(Vtmp[i] - Vo[i])*(Vtmp[i] - Vo[i]);
            
            Gbx[(i+1)%80] -= Ky[0]*(Y_tmp - Yo[0])*(By[(i+2)%80] - By[i])/2.0;
            Gby[(i+1)%80] -= Ky[0]*(Y_tmp - Yo[0])*(Bx[i] - Bx[(i+2)%80])/2.0;
            
            Gbx[(i+1)%80] -= Ky[1]*(By[(i+2)%80] - By[i]);
            Gby[(i+1)%80] -= Ky[1]*(Bx[i] - Bx[(i+2)%80]);
        }
        
        if (Energy_old < Energy[0]){
            Is_done[0] = 0;
            break;}
        else{
            Energy_old = Energy[0];}
        
        /* Move the verticies along the gradient with step size beta */
        for (i=0;i<80;i++){
            Ax[i] += Gax[i]*beta[0];Ay[i] += Gay[i]*beta[0];Bx[i] += Gbx[i]*beta[0];By[i] += Gby[i]*beta[0];}
        
        
        /* Apply boundary conditions */
        for (i=0;i<80;i++){
            /* Outer verticies cannot go outside circle of R_out */
            R_tmp = sqrt(Ax[i]*Ax[i] + Ay[i]*Ay[i]);
            if (R_tmp > R_out[0]){
                Ax[i] *= R_out[0]/R_tmp; Ay[i] *= R_out[0]/R_tmp;}
            R_tmp = sqrt(Bx[i]*Bx[i] + By[i]*By[i]);
            if (R_tmp > R_in[0]){
                Bx[i] *= R_in[0]/R_tmp; By[i] *= R_in[0]/R_tmp;}
            
            /* Middle boundary */
            if (i<=39 && Ax[i] < 0){
                Ax[i] = 0;}
            else if (i>=40 && Ax[i] > 0){
                Ax[i] = 0;}
        }
    }
    
    return;
}

void mexFunction( int nlhs, mxArray *plhs[], 
		  int nrhs, const mxArray *prhs[] )
     
{ 
    double *Gax,*Gay,*Gbx,*Gby,*Is_done; 
    double *Ax,*Ay,*Bx,*By,*Ka_lin,*Ka_con,*Ao_con,*Ka,*Ao,*Kl,*Lo,*Kb,*Kb_lin,*Bo,*Kv,*Vo,*Ky,*Yo,*R_out,*R_in,*beta,*n_steps,*Energy;
    mwSize m,n; 
    
    /* Check for proper number of arguments */
    
    if (nrhs != 23) { 
	mexErrMsgTxt("Incorrect number of input arguments."); 
    } 
    if (nlhs != 5) {
	mexErrMsgTxt("Incorrect number of output arguments."); 
    } 
    
    m = mxGetM(prhs[0]); 
    n = mxGetN(prhs[0]);
    
    /* Create a matrix for the return argument */ 
    plhs[0] = mxCreateDoubleMatrix(m, n, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(m, n, mxREAL);
    plhs[2] = mxCreateDoubleMatrix(m, n, mxREAL);
    plhs[3] = mxCreateDoubleMatrix(m, n, mxREAL);
    plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL); 
    
    /* Assign pointers to the various parameters */ 
    Gax = mxGetPr(plhs[0]);
    Gay = mxGetPr(plhs[1]);
    Gbx = mxGetPr(plhs[2]);
    Gby = mxGetPr(plhs[3]);
    Is_done = mxGetPr(plhs[4]);
    
    
    Ax = mxGetPr(prhs[0]); 
    Ay = mxGetPr(prhs[1]);
    
    Bx = mxGetPr(prhs[2]); 
    By = mxGetPr(prhs[3]);
    
    Ka_lin = mxGetPr(prhs[4]);
    Ka_con = mxGetPr(prhs[5]);
    Ao_con = mxGetPr(prhs[6]);
    
    Ka = mxGetPr(prhs[7]);
    Ao = mxGetPr(prhs[8]);
    
    Kl = mxGetPr(prhs[9]);
    Lo = mxGetPr(prhs[10]);
    
    Kb = mxGetPr(prhs[11]);
    Kb_lin = mxGetPr(prhs[12]);
    Bo = mxGetPr(prhs[13]);
    
    Kv = mxGetPr(prhs[14]);
    Vo = mxGetPr(prhs[15]);
    
    Ky = mxGetPr(prhs[16]);
    Yo = mxGetPr(prhs[17]);
    
    R_out = mxGetPr(prhs[18]);
    R_in = mxGetPr(prhs[19]);
    
    beta = mxGetPr(prhs[20]);
    n_steps = mxGetPr(prhs[21]);
    Energy = mxGetPr(prhs[22]);
        
    /* Do the actual computations in a subroutine */
    CALC_GRAD_C_N_STEPS(Energy, Gax, Gay, Gbx, Gby, Ax, Ay, Bx, By, Ka_lin, Ka_con, Ao_con, Ka, Ao, Kl, Lo, Kb, Kb_lin, Bo, Kv, Vo, Ky, Yo, R_out, R_in, beta, n_steps, Is_done); 
    return;
    
}


