/*
This file is part of EZRide.
Copyright 2010 Vadim Biktashev & Andrew Foulkes.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published
by the Free Software Foundation, either version 3 of the License,
or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ezride.h"
#include <stdio.h>
#include <math.h>

static const Real zero   = 0.;

/* ========================================================================= */

Real LX2, LY2, cx=0.0, cy=0.0, omega, Utip, Vtip;
Real stab_cx, stab_cy, stab_om, dt_o_2h;
Real cx_prev, cy_prev, om_prev, diff_cx, diff_cy, diff_om;
int  NX2, NY2, x_inc, y_inc;
int  cxdiff, cydiff, omdiff, istep, plot_step;

/* ========================================================================= */

void Step1 (void)
{
  int  i,j;
  Real conv1,conv2,Ux,Uy,Vx,Vy;
  Real x,y;
  
  Real dudt,dvdt,du1dt,dudx,du1dx,dvdx,dudy,du1dy,dvdy;
  Real small=1.e-9;
  Real denom=1;
  Real det;
  Real Ut,U1x,U1y,U1t,Vt;

  /* Find the spatial derivatives needed to determine the comoving frame displacement */
  /* 2nd order upstream schemes, directions determined by previous values of cx, cy   */
  if (cx>0.0) {
    Ux   =  -U(NX2+2       ,NY2       ) + (4*U(NX2+1       ,NY2       )) - (3*U(NX2       ,NY2       ));
    Vx   =  -V(NX2+2       ,NY2       ) + (4*V(NX2+1       ,NY2       )) - (3*V(NX2       ,NY2       ));
    U1x  =  -U(NX2+x_inc+2 ,NY2+y_inc ) + (4*U(NX2+x_inc+1 ,NY2+y_inc )) - (3*U(NX2+x_inc ,NY2+y_inc ));
  } else if (cx< 0.0) {
    Ux   =   U(NX2-2       ,NY2       ) - (4*U(NX2-1       ,NY2       )) + (3*U(NX2       ,NY2       ));
    Vx   =   V(NX2-2       ,NY2       ) - (4*V(NX2-1       ,NY2       )) + (3*V(NX2       ,NY2       ));
    U1x  =   U(NX2+x_inc-2 ,NY2+y_inc ) - (4*U(NX2+x_inc-1 ,NY2+y_inc )) + (3*U(NX2+x_inc ,NY2+y_inc ));
  } else {
    Ux   =   U(NX2+1,NY2) - U(NX2-1,NY2);
    Vx   =   V(NX2+1,NY2) - V(NX2-1,NY2);
    U1x  =   U(NX2+x_inc+1,NY2+y_inc) - U(NX2+x_inc-1,NY2+y_inc);
  }

  if (cy>0.0) {
    Uy   =  -U(NX2       ,NY2+2       ) + (4*U(NX2       ,NY2+1       )) - (3*U(NX2       ,NY2       ));
    Vy   =  -V(NX2       ,NY2+2       ) + (4*V(NX2       ,NY2+1       )) - (3*V(NX2       ,NY2       ));
    U1y  =  -U(NX2+x_inc ,NY2+y_inc+2 ) + (4*U(NX2+x_inc ,NY2+y_inc+1 )) - (3*U(NX2+x_inc ,NY2+y_inc ));
  } else if (cy<0.0) {
    Uy   =   U(NX2       ,NY2-2       ) - (4*U(NX2       ,NY2-1       )) + (3*U(NX2       ,NY2       ));
    Vy   =   V(NX2       ,NY2-2       ) - (4*V(NX2       ,NY2-1       )) + (3*V(NX2       ,NY2       ));
    U1y  =   U(NX2+x_inc ,NY2+y_inc-2 ) - (4*U(NX2+x_inc ,NY2+y_inc-1 )) + (3*U(NX2+x_inc ,NY2+y_inc ));
  } else {
    Uy   =   U(NX2,NY2+1) - U(NX2,NY2-1);
    Vy   =   V(NX2,NY2+1) - V(NX2,NY2-1);
    U1y  =   U(NX2+x_inc,NY2+y_inc+1) - U(NX2+x_inc,NY2+y_inc-1);
  }
  
  /* Time derivatives */
  Ut  =  Utip - U(NX2      ,NY2      );
  Vt  =  Vtip - V(NX2      ,NY2      );
  U1t =  Utip - U(NX2+x_inc,NY2+y_inc);

  dudt  = Ut/dt;			/*du(x0,y0)/dt*/
  dudx  = Ux/(2*grid_h);                /*du(x0,y0)/dx*/
  dudy  = Uy/(2*grid_h);		/*du(x0,y0)/dy*/
  dvdt  = Vt/dt;			/*dv(x0,y0)/dt*/
  dvdx  = Vx/(2*grid_h);		/*dv(x0,y0)/dx*/
  dvdy  = Vy/(2*grid_h);		/*dv(x0,y0)/dy*/
  du1dt = U1t/dt;			/*du(x0+inc,y0)/dt*/
  du1dx = U1x/(2*grid_h);		/*dv(x0+inc,y0)/dx*/
  du1dy = U1y/(2*grid_h);		/*dv(x0+inc,y0)/dy*/

  /* Now find the FoR displacement by solving the linear system for pinning conditions at the next time layer */
  det = (dudx*dvdy) - (dudy*dvdx);

  cy    = (fabs(det)>small)? (((dudx*dvdt)-(dudt*dvdx))/det) : zero;
  cx    = (fabs(det)>small)? (((dudt*dvdy)-(dvdt*dudy))/det) : zero;

  if (cx >  stab_cx) cx =  stab_cx;
  if (cx < -stab_cx) cx = -stab_cx;
        
  if (cy >  stab_cy) cy =  stab_cy;
  if (cy < -stab_cy) cy = -stab_cy;
  
  denom = (x_inc*grid_h*du1dy) - (y_inc*grid_h*du1dx);
  omega = (fabs(denom)>small) ? ((du1dt-(cx*du1dx)-(cy*du1dy))/denom) : zero;
  if (omega >  stab_om) omega = zero;
  if (omega < -stab_om) omega = zero;
  if (V(x_inc+NX2,y_inc+NY2) > Vtip) omega = zero;

  /* Now perform the "convection" part-step */    

  for (i=1;i<=NX;i++){
    for (j=1;j<=NY;j++){
      x = (i*grid_h)-LX2;
      y = (j*grid_h)-LY2;
      conv1 = cx - (y*omega);
      conv2 = cy + (x*omega);
      
      if(conv1>0) {
	if (i==NX) {
	  Ux = -U(NX-2,j) + (4*U(NX-1,j))-(3*U(NX,j));
	  Vx = -V(NX-2,j) + (4*V(NX-1,j))-(3*V(NX,j));
	} else if (i==NX-1) {
	  Ux = -U(NX-1,j) + (4*U(NX,j))-(3*U(NX-1,j));
	  Vx = -V(NX-1,j) + (4*V(NX,j))-(3*V(NX-1,j));
	} else {
	  Ux = -U( i+2,j) + (4*U(i+1,j))-(3*U(i,j));
	  Vx = -V( i+2,j) + (4*V(i+1,j))-(3*V(i,j));
	}
      } else {
	if (i==1) {
	  Ux = U(  3,j) - (4*U(2,j)) + (3*U(1,j));
	  Vx = V(  3,j) - (4*V(2,j)) + (3*V(1,j));
	} else if (i==2) {
	  Ux = U(  2,j) - (4*U(1,j)) + (3*U(2,j));
	  Vx = V(  2,j) - (4*V(1,j)) + (3*V(2,j));
	} else {
	  Ux = U(i-2,j) - (4*U(i-1,j)) + (3*U(i,j));
	  Vx = V(i-2,j) - (4*V(i-1,j)) + (3*V(i,j));
	}
      }
      
      if (conv2>0) {
	if (j==NY) {
	  Uy = -U(i,NY-2) + (4*U(i,NY-1)) - (3*U(i,NY));
	  Vy = -V(i,NY-2) + (4*V(i,NY-1)) - (3*V(i,NY));
	} else if (j==NY-1) {
	  Uy = -U(i,NY-1) + (4*U(i,  NY)) - (3*U(i,NY-1));
	  Vy = -V(i,NY-1) + (4*V(i,  NY)) - (3*V(i,NY-1));
	} else {
	  Uy = -U(i, j+2) + (4*U(i, j+1)) - (3*U(i,j));
	  Vy = -V(i, j+2) + (4*V(i, j+1)) - (3*V(i,j));
	}
      } else {
	if (j==1) {
	  Uy = U(i,  3) - (4*U(i,  2)) + (3*U(i,1));
	  Vy = V(i,  3) - (4*V(i,  2)) + (3*V(i,1));
	} else if (j==2) {
	  Uy = U(i,  2) - (4*U(i,  1)) + (3*U(i,2));
	  Vy = V(i,  2) - (4*V(i,  1)) + (3*V(i,2));
	} else {
	  Uy = U(i,j-2) - (4*U(i,j-1)) + (3*U(i,j));
	  Vy = V(i,j-2) - (4*V(i,j-1)) + (3*V(i,j));
	}
      }

      U1(i,j) = U(i,j) + (dt_o_2h*((conv1*Ux) + (conv2*Uy)));
      V1(i,j) = V(i,j) + (dt_o_2h*((conv1*Vx) + (conv2*Vy)));

    }
  }

  for (i=1;i<=NX;i++){
    for (j=1;j<=NY;j++){
      U(i,j) = U1(i,j);
      V(i,j) = V1(i,j);
    }
  }

  /* Counters that can be used to register convergence to relative equilbrium */
  if(istep%plot_step==0){
    if(omega!=zero){
      diff_cx = cx-cx_prev;
      diff_cy = cy-cy_prev;
      diff_om = omega-om_prev;
      
      if (diff_cx==0) cxdiff += 1;
      else           cxdiff  = 0;
      
      if (diff_cy==0) cydiff += 1;
      else           cydiff  = 0;
      
      if (diff_om==0) omdiff += 1;
      else           omdiff  = 0;}
    
    cx_prev=cx;
    cy_prev=cy;
    om_prev=omega;
  }
}
/* ========================================================================= */