#include "cm4model.h"
#include <stdlib.h>
#include <math.h>



const double dPi  = 3.141592653589793238462643383279502884197169399;


CM4::CM4(bool bRegister) :
  ContactModel(ulModelCM4, bRegister),
     knc(0.0), knt(0.0), ks(0.0), FTmax(0.0),
     FSmax(0.0), fric(0.0), rfric(0.0), uplim(0.0), broken(0.0),
     softened(0.0), uplas(0.0) {
  SetDelete(false);
}


const char *CM4::Name(void) const {
  return("softening");
}


const char **CM4::PropNames(void) const {
  static const char *strKey[] = {
    "sof_knc", "sof_knt", "sof_ks", "sof_ftmax", "sof_fsmax",
    "sof_fric", "sof_rfric", "sof_uplim", "sof_broken",
    "sof_softened", "sof_uplas",
     0
  };
  return(strKey);
}


double CM4::ReturnProp(int n) const {
  switch (n) {
    case  0:  return knc;
    case  1:  return knt;
    case  2:  return ks;
    case  3:  return FTmax;
    case  4:  return FSmax;
    case  5:  return fric;
    case  6:  return rfric;
    case  7:  return uplim;
    case  8:  return broken;
    case  9:  return softened;
    case 10:  return uplas;
    default:  return 0.0;
  }
}


void CM4::AcceptProp(int n, double v) {
  switch (n) {
    case  0:  knc      = v;        break;
    case  1:  knt      = v;        break;
    case  2:  ks       = v;        break;
    case  3:  FTmax    = v;        break;
    case  4:  FSmax    = v;        break;
    case  5:  fric     = v;        break;
    case  6:  rfric    = v;        break;
    case  7:  uplim    = v;        break;
    case  8:  broken   = v;        break;
    default: break;
  }
}


const char *CM4::PreCycle(PropBlock &) {
  rfric = rfric < fric ? rfric : fric;
  if (broken == 0.0) {
    if (knt == 0.0) {
      knt = knc;
    }
    if (FTmax == 0.0 && FSmax == 0.0) {
        broken = 1.0;
    }
    double uplimT = FTmax / knt;
    double uplimS = FSmax / ks;
    double uplimG = uplimT < uplimS ? uplimT : uplimS;
    uplim  = uplim > uplimG ? uplim : uplimG;
  }
  return(0);
}


//------------ built-in, user-defined contact model "softening" -----------
void CM4::FDlaw(FdBlock& fb, char cDim) {
  if (cDim==2) { // ... 2D section ...
    double dun;
    double kst = ks  * fb.tdel;
    double dus = fb.u_dot_s * kst;
    if (fb.n_force > 0.0) {          // COMPRESSIVE
      double knct = knc  * fb.tdel;
      dun = fb.u_dot_n * knct;
		}
		else {
      double kntt = knt  * fb.tdel;
      dun = fb.u_dot_n * kntt;
		}
    if ((broken != 1.0) || ((broken == 1.0) && (fb.u_n > 0.0))) {
      fb.n_force -= dun;
      fb.s_force -= dus;
		}
		else {
      fb.n_force = 0.;
      fb.s_force = 0.;
		}
    if (fb.n_force > 0.0) {          // COMPRESSIVE
      double Dcoef;
      double max_s_force = fb.n_force * (fric * (1. - softened) + rfric * softened) + FSmax * (1.0 - softened); // Slip check
      double max_rs_force = fb.s_force < 0.0 ? -fabs(fb.n_force*rfric) : fabs(fb.n_force*rfric);  // Residual force
      double sfmag = fabs(fb.s_force);
      if (sfmag > max_s_force) {
        Dcoef = broken == 1 ? 0. : FSmax/uplim;
        double duplas  = (sfmag - max_s_force)/(ks - Dcoef);
        fb.s_force -= fb.s_force < 0.0 ? -fabs(ks*fabs(duplas)) : fabs(ks*fabs(duplas));
        if (fb.s_force * max_rs_force < 0 ||  fabs(fb.s_force) < fabs(max_rs_force))
          fb.s_force = max_rs_force;
        uplas += fabs(duplas);
			}
      fb.knest = knc;
      fb.ksest = ks;
      fb.skip  = false;
		}
    else {                     // EXTENSILE
      if (broken == 1.0) {
        fb.n_force = 0.;
        fb.s_force = 0.;
			}
			else {
        double Fn = fb.n_force;
        double Fs = fb.s_force;
        double fmag  = sqrt(Fn * Fn + Fs * Fs);
        double alpha = 2.0 * acos(fabs(Fn / fmag)) / dPi;
        double Fmax  = (1.0 - alpha) * FTmax + alpha * FSmax;
        double max_force = Fmax * (1.0 - softened);
        if (fmag > max_force) {
          double CoefA   = knt * knt * Fn * Fn / (fmag * fmag) + ks * ks * Fs * Fs / (fmag * fmag)
                         - Fmax * Fmax / (uplim * uplim);
          double CoefB   = 2. * Fmax * Fmax * (1.0 - softened) / uplim - 2. * knt * Fn * Fn / fmag
                         - 2. * ks * Fs * Fs / fmag;
          double CoefC   = fmag * fmag - max_force * max_force;
          if (CoefA != 0. && (CoefB*CoefB-4.*CoefA*CoefC)>=0.) {
            double duplas  = 0.5*(-CoefB - sqrt(CoefB*CoefB-4.*CoefA*CoefC))/CoefA;
            fb.s_force -= fb.s_force < 0.0 ? -fabs(ks*fabs(duplas*Fs)/fmag) : fabs(ks*fabs(duplas*Fs)/fmag);
            if (Fs * fb.s_force < 0.) {
              fb.s_force = 0.;
            }
            fb.n_force -= fb.n_force < 0.0 ? -fabs(knt*fabs(duplas*Fn)/fmag) : fabs(knt*fabs(duplas*Fn)/fmag);
            if (fb.n_force > 0.) {
              fb.n_force = 0.;
            }
            uplas += fabs(duplas);
          }
          else {
            fb.s_force = 0.;
            fb.n_force = 0.;
            uplas = uplim;
          }
        }
			}
      fb.knest = knt;
      fb.ksest = ks;
      fb.skip  = false;
		}
    if (broken != 1.0) {
      if ((softened == 0.) & (uplas != 0.)) {
        fb.bfishc_brokens = true;
			}
      softened = uplas/uplim < 1.0 ? uplas/uplim : 1.0;
      if (softened == 1.0) {
        broken = 1.0;
        SetDelete(true);
        fb.bfishc_brokenn = true;
        fb.bflag = false;
        fb.broken = true;
			}
		}
	}
	else { // ... 3D section ...
    double dun;
    double kst  = ks  * fb.tdel;
    UMdvect dus   = fb_u_dot_s * kst;
    if (fb.n_force > 0.0) {          // COMPRESSIVE
      double knct = knc  * fb.tdel;
      dun = fb.u_dot_n * knct;
		}
		else {
      double kntt = knt  * fb.tdel;
      dun = fb.u_dot_n * kntt;
		}
    if ((broken != 1.0) || ((broken == 1.0) && (fb.u_n > 0.0))) {
      fb.n_force -= dun;
      fb_s_force= fb_s_force- dus;
		}
		else {
      fb.n_force = 0.;
      fb_s_force.Fill(0.0);
		}
    if (fb.n_force > 0.0) {          // COMPRESSIVE
      double Dcoef;
      double max_s_force = fb.n_force * (fric * (1. - softened) + rfric * softened) + FSmax * (1.0 - softened); // Slip check
      double dX = fb.pts_force->x;
      double dY = fb.pts_force->y;
      double dZ = fb.pts_force->z;
      double sfmag = sqrt(dX*dX + dY*dY + dZ*dZ);
      UMdvect sunit  = fb_s_force/sfmag;
      UMdvect max_rs_force = sunit * fb.n_force * rfric;       // Residual force
      if (sfmag > max_s_force) {
        Dcoef = broken == 1 ? 0.0 : FSmax/uplim;
        double duplas  = (sfmag - max_s_force)/(ks - Dcoef);
        fb_s_force= fb_s_force - sunit*ks*fabs(duplas);
        double dX = fb.pts_force->x;
        double dY = fb.pts_force->y;
        double dZ = fb.pts_force->z;
        double sfmag = sqrt(dX*dX + dY*dY + dZ*dZ);
        dX = max_rs_force.x;
        dY = max_rs_force.y;
        dZ = max_rs_force.z;
        double rfmag = sqrt(dX*dX + dY*dY + dZ*dZ);
        if ((fb_s_force| max_rs_force) < 0. ||  sfmag < rfmag )
            fb_s_force= max_rs_force;
        uplas += fabs(duplas);
			}
      fb.knest = knc;
      fb.ksest = ks;
      fb.skip  = false;
		}
    else {                     // EXTENSILE
      if (broken == 1.0) {
        fb.n_force = 0.;
        fb_s_force.Fill(0.0);
			}
			else {
        double Fn = fb.n_force;
        double dX = fb.pts_force->x;
        double dY = fb.pts_force->y;
        double dZ = fb.pts_force->z;
        double Fs = sqrt(dX*dX + dY*dY + dZ*dZ);
        UMdvect sunit  = fb_s_force/Fs;
        double fmag  = sqrt(Fn * Fn + Fs * Fs);
        double alpha = 2.0 * acos(fabs(Fn / fmag)) / dPi;
        double Fmax  = (1.0 - alpha) * FTmax + alpha * FSmax;
        double max_force = Fmax * (1.0 - softened);
        if (fmag > max_force) {
          double CoefA   = knt * knt * Fn * Fn / (fmag * fmag) + ks * ks * Fs * Fs / (fmag * fmag)
                         - Fmax * Fmax / (uplim * uplim);
          double CoefB   = 2. * Fmax * Fmax * (1.0 - softened) / uplim - 2. * knt * Fn * Fn / fmag
                         - 2. * ks * Fs * Fs / fmag;
          double CoefC   = fmag * fmag - max_force * max_force;
          if (CoefA != 0. && (CoefB*CoefB-4.*CoefA*CoefC)>=0.) {
            double duplas  = 0.5*(-CoefB - sqrt(CoefB*CoefB-4.*CoefA*CoefC))/CoefA;
            fb_s_force= fb_s_force-sunit*(ks*fabs(duplas*Fs)/fmag);
            if (Fs*(fb_s_force| sunit) < 0.) {
              fb_s_force.Fill(0.0);
            }
            fb.n_force -= fb.n_force < 0.0 ? -fabs(knt*fabs(duplas*Fn)/fmag) : fabs(knt*fabs(duplas*Fn)/fmag);
            if (fb.n_force > 0.) {
              fb.n_force = 0.;
            }
            uplas += fabs(duplas);
          }
          else {
            fb_s_force.Fill(0.0);
            fb.n_force = 0.;
            uplas = uplim;
          }
        }
			}
      fb.knest = knt;
      fb.ksest = ks;
      fb.skip  = false;
		}
    if (broken != 1.0) {
      if ((softened == 0.) & (uplas != 0.)) {
        fb.bfishc_brokens = true;
			}
      softened = uplas/uplim < 1.0 ? uplas/uplim : 1.0;
      if (softened == 1.0) {
        broken = 1.0;
        SetDelete(true);
        fb.bfishc_brokenn = true;
        fb.bflag  = false;
        fb.broken = true;
			}
		}
  }
}


const char *CM4::SaveRestore(ModelSaveObject *mso) {
  const char *str = ContactModel::SaveRestore(mso);
  if (str) return(str);
  mso->Initialize(11,0,1);
  mso->Save( 0, delete_flag);
  mso->Save( 0, knc);
  mso->Save( 1, knt);
  mso->Save( 2, ks);
  mso->Save( 3, FTmax);
  mso->Save( 4, FSmax);
  mso->Save( 5, fric);
  mso->Save( 6, rfric);
  mso->Save( 7, uplim);
  mso->Save( 8, uplas);
  mso->Save( 9, broken);
  mso->Save(10, softened);
  return(0);
}



/* EoF */
