#include "hysdmodel.h"
#include <math.h>

static CM_HysDamp cm_hysdamp(true);



CM_HysDamp::CM_HysDamp(bool bRegister) :
  ContactModel(ulModelCM_HysDamp, bRegister),
                     kn_load(0.0), kn_unload(0.0), kn_m(0.0),
                     ks(0.0), fric(0.0),
                     hys_nstr(0.0), hys_sstr(0.0), type_gobj2(BALLs),
                     sp_damp(0.8),        // rest. coeff. = 0.9
                     bNtension(false),    // with tensile force
										 bInheritProp(false), // NOT inherit
                     u_old(0.0), u_zero(0.0), u_zeroo(0.0), u_max(0.0), n_force_old(0.0) {
  SetDelete(true);
}


const char *CM_HysDamp::Name(void) const {
  return("hysdamp");
}


const char **CM_HysDamp::PropNames(void) const {
  static const char *strKey[] = {
    "hys_knm", "hys_dampn", "hys_ks", "hys_fric",
    "hys_nstr", "hys_sstr", "hys_notension", "hys_inheritprop", 0
  };
  return(strKey);
}


ContactModel *CM_HysDamp::Clone(PropBlock *pb) const {
  CM_HysDamp *cm_hd = new CM_HysDamp;
  if(pb) cm_hd->SetProp(*pb);
  return(cm_hd);
}


double CM_HysDamp::ReturnProp(int n) const {
  switch (n) {
    case 0 : return kn_m;
    case 1 : return sp_damp;
    case 2 : return ks;
    case 3 : return fric;
    case 4 : return hys_nstr;
    case 5 : return hys_sstr;
    case 6 : return bNtension ? 1.0 : 0.0;
    case 7 : return bInheritProp ? 1.0 : 0.0;
    default: return 0.0;
  }
}


void CM_HysDamp::AcceptProp(int n, double v) {
  switch (n) {
    case 0:  kn_m      = v;                        break;
    case 1:  sp_damp   = v;                        break;
    case 2:  ks        = v;                        break;
    case 3:  fric      = v;                        break;
    case 4:  hys_nstr  = v;                        break;
    case 5:  hys_sstr  = v;                        break;
    case 6:  bNtension = v!=0.0 ? true : false;    break;
    case 7:  bInheritProp = v!=0.0 ? true : false; break;
    default:                                       break;
  }
  SetKn();
}


const char *CM_HysDamp::PreCycle(PropBlock &pb) {
	if(bInheritProp) SetProp(pb);
  if(!bNtension && (sp_damp>1.0 || sp_damp<0.4))
    return("range of damping factor : 0.4 <= damp_hysdamp <= 1");
  if( bNtension && (sp_damp>1.0 || sp_damp<0.05))
    return("range of damping factor : 0.05 <= damp_hysdamp <= 1");
  return(0);
}


void CM_HysDamp::SetProp(PropBlock &pb) {
  kn_m       = pb.kn_c;
  ks         = pb.ks_c;
  fric       = pb.fric_c;
  hys_nstr   = pb.n_strength;
  hys_sstr   = pb.s_strength;
  type_gobj2 = pb.type_gobj2;
  SetKn();
}


void CM_HysDamp::SetKn(void) {
  kn_load   = 2.0*sp_damp*kn_m/(1.0+sp_damp);
  kn_unload = 2.0*kn_m/(1.0+sp_damp);
}


//------------ user-defined contact model "HysDamp" -----------
void CM_HysDamp::FDlaw(FdBlock& fb, char cDim) { // Force/displacement law, HystereticDamping
  if (fb.u_n<=0.0 && !fb.bflag) {
    fb.n_force = 0.0;
    if (cDim==2)
      fb.s_force  = 0.0;
		else 
      fb_s_force.Fill(0.0);
    u_old = 0.0;
    u_zero = 0.0;
    u_zeroo = 0.0;
    u_max = 0.0;
    n_force_old = 0.0;
    fb.knest = 0.0;
    fb.ksest = 0.0;
    fb.skip  = true;
  }
  else {
    if(!bNtension) {   // with tensile force
      // fb.u_n > 0 or exists Contact-Bond
      if(fb.u_n - u_old >= 0.0 && n_force_old >= 0.0
         || fb.u_n - u_old <= 0.0 && n_force_old <= 0.0) {  // loading
        fb.n_force = kn_load*(fb.u_n - u_zero);
        u_max = fb.u_n;
        u_zeroo = u_zero;
      }
      else {                                                // unloading
        fb.n_force = kn_unload*(fb.u_n - u_max) - kn_load*(u_zeroo - u_max);
				if(kn_load!=0.0) u_zero =  fb.u_n - fb.n_force/kn_load;
				else             u_zero =  fb.u_n;
        if(fb.n_force * n_force_old < 0.0) {     // transition into loading
          u_zero = (kn_unload*u_max + kn_load*(u_zeroo-u_max)) / kn_unload;
          fb.n_force = kn_load*(fb.u_n - u_zero);
          u_max = fb.u_n;
          u_zeroo = u_zero;
        }
      }
      n_force_old = fb.n_force;
    }
    else {   // without tensile force
      if(fb.u_n - u_old >= 0.0) {
        fb.n_force = kn_load*(fb.u_n - u_zero);
        u_max = fb.u_n;
        u_zeroo = u_zero;
      }
      else {
        fb.n_force = kn_unload*(fb.u_n - u_max) - kn_load*(u_zeroo - u_max);
        if(fb.n_force < 0.0) fb.n_force = 0.0;
        if(kn_load!=0.0) u_zero =  fb.u_n - fb.n_force/kn_load;
				else             u_zero =  fb.u_n;
      }
    }
    u_old = fb.u_n;
    if (cDim==2)
      fb.s_force  -= fb.u_dot_s*ks*fb.tdel;
    else
      fb_s_force  -= fb_u_dot_s*ks*fb.tdel;
    if(fb.bflag)     // bonding check
      CheckBonding(fb, cDim);
    else {           // sliding check
      double max_s_force = fabs(fb.n_force * fric);
      if (cDim==2) { 
        double sfmag = fabs(fb.s_force);
        if(sfmag > max_s_force) {
          fb.s_force = fb.s_force * max_s_force / sfmag;
					fb.bSliding = true;
        }
      }
		  else {
        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);
        if(sfmag > max_s_force) {
          fb_s_force = fb_s_force * max_s_force / sfmag;
					fb.bSliding = true;
        }
			}
		}
    fb.knest = kn_unload;
    fb.ksest = ks;
    fb.skip  = false;
  }
}


void CM_HysDamp::CheckBonding(FdBlock &fb, char cDim) {
  if (type_gobj2==BALLs) {
		if (cDim==2) {  // 2D...
      if (-fb.n_force >= hys_nstr) {
        fb.bfishc_brokenn = true;
        fb.bflag   = false;
        fb.broken  = true;
        fb.n_force = 0.0;
        fb.s_force  = 0.0;
			}
			else if (fabs(fb.s_force) >= hys_sstr) {
        fb.bfishc_brokens = true;
        fb.bflag   = false;
        fb.broken  = true;
			}
    }
		else {  // 3D...
      if (-fb.n_force >= hys_nstr) {
        fb.bfishc_brokenn = true;
        fb.bflag   = false;
        fb.broken  = true;
        fb.n_force = 0.0;
        fb_s_force.Fill(0.0);
      }
      else {
        double dX = fb.pts_force->x;
        double dY = fb.pts_force->y;
        double dZ = fb.pts_force->z;
        double dMag = sqrt(dX*dX + dY*dY + dZ*dZ);
        if (dMag >= hys_sstr) {
          fb.bfishc_brokens = true;
          fb.bflag   = false;
          fb.broken  = true;
				}
			}
    }
  }
}


const char *CM_HysDamp::SaveRestore(ModelSaveObject *mso) {
  const char *str = ContactModel::SaveRestore(mso);
  if (str) return(str);
  mso->Initialize(13,0,3);
  mso->Save( 0, delete_flag);
  mso->Save( 1, bNtension);
  mso->Save( 2, bInheritProp);
  mso->Save( 0, kn_load);
  mso->Save( 1, kn_unload);
  mso->Save( 2, kn_m);
  mso->Save( 3, ks);
  mso->Save( 4, fric);
  mso->Save( 5, hys_nstr);
  mso->Save( 6, hys_sstr);
  mso->Save( 7, sp_damp);
  mso->Save( 8, u_old);
  mso->Save( 9, u_zero);
  mso->Save(10, u_zeroo);
  mso->Save(11, u_max);
  mso->Save(12, n_force_old);
  return(0);
}



/* EOF */