(*                NDTools

           Techno-Sciences, Inc.
           10001 Derekwood Lane, Suite 204
           Lanham, MD 20706
           (301) 577-6000
           
           
Copyright © 1996, 1998 Techno-Sciences Incorporated
All Rights Reserved          
           
*)
spell1 = (Head[General::spell1] === $Off);
spell = (Head[General::spell] === $Off);
Off[General::spell1];
Off[General::spell]; 

If[$VersionNumber===2.,BeginPackage["ProPac`NDTools`",{"Calculus`DiracDelta`"}]];
If[$VersionNumber===3.,BeginPackage["ProPac`NDTools`",{"Calculus`DiracDelta`"}]];
If[$VersionNumber===4.,BeginPackage["ProPac`NDTools`",{"ProPac`DiracDeltaExtensions`"}]];

(* ********************************************************************** *)
(* ********************************************************************** *)


(* Function Usage Statements *)

Sgn::usage=
"Sgn[x] is the same Sign[x] but it evaluates in terms of UnitStep functions when
x is a symbol."

Sat::usage=
"Sat[x] is the fuction:\n
\n
            Sat[x]=-1 for x<1, x for -1<=x<=1, 1 for x>1\n
\n
It evaluates in terms of UnitStep functions.\n
"

Sig::usage=
"Sig[x,k,a,b] is the function:\n
\n
         Sig[x,k,a,b]=-k for x<a, 0 for a<=x<=b, k for x>b\n
\n
x,k,a,b can be lists of the same dimension, in which case \n
the Sig returns a vector of funtions threaded through the\n
argument lists. The function evaluates in terms of Sign functions.\n
"

BacklashPotential::usage=
"BacklashPotential[P,e,x] returns the backlash potential function\n
associated with a Hertz impact potential function P (must be defined \n
as a pure function), a backlash parameter e, and a coordinate x. Note\n
that it may be appropriate to use BacklashPotential in conjunction with\n
SpringPotential."

BacklashForce::usage=
"BacklashForce[P,e,x] returns the force associated with backlash element as\n
defined in BacklashPotential."

JointFrictionPotential::usage=
"JointFrictionPotential[v_,Fv_,Fc_,Fs_,vs_] assembles a dissipation\n
function of Lur'e type for a joint that involves viscous, Coulomb and\n
Stribeck effects. The potential function has meaning if the arguments\n
are functions of the joint variables as described below. The function is listable.\n
\n
The function calls:\n
\n
 JointFrictionPotential[v_,Fv_,Fc_,Fs_,vs_] or JointFrictionPotential[v_,Fv_,Fc_]\n
\n
return the dissipation function for a joint friction component. The\n
'velocity' v must be an expression involving joint quasi-velocities.\n
It may be a function of the coordinates as well.\n
\n
The friction parameters are:\n
\n
          Fv-viscous friction coefficient\n
          Fc-Coulomb friction coefficient,\n
          Fs-Static friction,\n
          vs-Stribeck velocity.\n
\n
These coefficients may be functions of the joint coordinates."

SimplifyDiracDelta1::usage=
"SimplifyDiracDelta1[expr] simplifies Dirac Delta functions that appear when
differentiating continuous but non-smooth functions. It performs other 
simplifications associated with Abs or Sign functions. expr may be an expression 
or list of expressions."

UnitStep2Sign::usage=
"UnitStep2Sign[expr] replaces all UnitStep functions by\n
corresponding expressions involving the Sign function\n 
and performs some simplification."

(*  Functions *)

Begin["`private`"]

Sgn[x_]:=UnitStep[x]-UnitStep[-x];
Sat[x_]:=x-(x-1)*UnitStep[x-1]+(-x-1)*UnitStep[-x-1];
Sig[x_,k_,a_,b_]:=k Simplify[UnitStep2Sign[UnitStep[x-b]-UnitStep[a-x]]];
Sig[x_List,k_List,a_List,b_List]:=Module[{},
		If[(Length[x]=!=Length[k])||(Length[k]=!=Length[a])||(
          Length[a]=!=Length[b]),Print["Incompatible list dimensions!"];
      Return[{}]];
		MapThread[Sig[#1,#2,#3,#4]&,{x,k,a,b}]
		]

(*   Rules for Derivative of Abs  *)

Unprotect[Derivative, Abs, Sign, Sqrt, Simplify]

Derivative[n_Integer?Positive][Sign] := Derivative[n][Sgn]
Simplify/: Simplify[expr_List] :=Map[Simplify,expr]/;!FreeQ[expr,Abs]
Simplify/: Simplify[expr_] :=
  Module[{k0,A,B, list, noab,ab, abcases, infy, xx,      
     AbsFreeQ, simplified=False},
     AbsFreeQ[z_] := FreeQ[z,Abs];
     If[Head[expr] =!= Plus,
        k0 = 0;A =expr,
        (* avoid expanding terms FreeQ of Abs *)
        list = Apply[List, expr];
        noab = Cases[list, z_ /;AbsFreeQ[z]];
        ab = Cases[list, z_ /; !AbsFreeQ[z]];
        k0 = Apply[Plus, noab];A =Apply[Plus, ab]
     ];
     infy = (Head[Power::infy] === $Off);
     Off[Power::infy];
     abcases = Cases[A, 1/Abs[___], 2];
     A=Collect[A,abcases];
     While[MatchQ[A, (b_.+((a_.) (x_^n_.) (c_.)/(Abs[x_])) (d_.))/;
      (xx= b+a c x^(n-1) Sign[x] d;(x=!=0)&&IntegerQ[n]&&(n>=0))],
     simplified = True; A=xx];
     While[MatchQ[A, (b_.+((a_.) /(Abs[x_])) (d_.))/;
      (xx=b+ a  x^(-1) Sign[x] d;(x=!=0))],
     simplified = True; A=xx];
     If[!infy, On[Power::infy]];
     B=If[simplified,
          A + k0,
          expr];
     B
  ]/;!FreeQ[expr,Abs]
Abs/: Derivative[n_Integer?Positive][Abs] := Derivative[n-1][Sgn]
Abs/: Integrate[Abs[x_], x_] := x Abs[x]/2
Abs/: Abs[y_] x_^n_.:=Module[{a},a=Cases[{y},a_./x->a][[1]];
  Abs[a] Sign[x] x^(n-1)  
  ]/;((x=!=0)&&MatchQ[y,a_./x]&&FreeQ[a,x]&&IntegerQ[n]&&(n>=1))
Abs/: Abs[x_]/x_:=Sign[x]
Abs/: Abs[a_/b_]:=Abs[a]/Abs[b]/;(b=!=0)
Sign/: Integrate[Sign[x_], x_] := Abs[x]
Sign/: x_ Sign[x_]:=Abs[x]
Sign/: Sign[Abs[x_]]:=1
Sign/: Abs[x_] Sign[y_] := x /;MatchQ[x,y]
Sign/: Abs[x_] Sign[y_] := -x/;MatchQ[x,-y]
Sign/: Sign[y_] Abs[x_]^n_:= Module[{pcd,a,b},
  pcd=PolynomialGCD[x,y];
  a=Simplify[y/pcd];
  b=Simplify[x/pcd];
  Abs[b]^n Sign[a] x Abs[x]^(n-1)
  ]/;((!NumericQ[PolynomialGCD[x,y]])&&IntegerQ[n]&&(n>1))
Sign/:Sign[x_]^n_:=Sign[x]/;OddQ[n]
Sign/:Sign[x_]^n_:=1/;EvenQ[n]
Sign/: Sign[x_] Sign[a_. x_ b_.] := Sign[a b];
Sign/:Sign[a_. x^n b_.]:=Sign[a b]/;EvenQ[n];
Sign/:Sign[a_. x^n b_.]:=Sign[a x b]/;OddQ[n];
Sqrt/:Sqrt[x_]:=Sqrt[Together[x]]/;(Head[x]===Plus)&&(Denominator[Together[x]]=!=1)
Sqrt/:Sqrt[a_/(b_  x_ Abs[x_])]:=Sqrt[Sign[x] a/b]/Abs[x]/;(x=!=0)&&(b=!=0)
Sqrt/:Sqrt[a_/(x_)]:=Sqrt[Sign[x] a]/Sqrt[Abs[x]]/;(x=!=0)
Sqrt/:Sqrt[a_/(b_ Abs[x_]^2)]:=Sqrt[a/b]/Abs[x]/;(x=!=0)&&(b=!=0)
Sqrt/:Sqrt[Abs[x_]^2]:=Abs[x]
Sqrt/:Sqrt[Abs[x_^2]]:=Abs[x]
Sqrt/:Sqrt[Abs[x_]]:=
  Module[{a},a=Cases[{Simplify[x]},z_.*y_^2->{z,y}][[1]];
  Sqrt[a[[1]]]*Abs[a[[2]]]
  ]/;MatchQ[Simplify[x],z_.*y_^2]
Protect[Abs, Sign, Sqrt, Simplify]      (* restore protection of system symbols *)

BacklashPotential[P_,e_,x_]:=P[Abs[x]-e]*UnitStep[Abs[x]-e]
BacklashForce[P_,e_,x_]:=SimplifyDiracDelta1[D[BacklashPotential[P,e,x],x]]

JointFrictionPotential[v_,Fv_,Fc_,Fs_,vs_]:=Module[{Viscous, Coulomb,Stribeck,Fs1=Fs},
      If[Fs<Fc,Fs1=Fc;Print["Static Friction Must be at Least" Fc]];
      Viscous=Fv*v^2/2;
      Coulomb=Fc*v*Sign[v];
      If[vs===0,Stribeck=0,
        Stribeck=(Fs1-Fc)*vs*Sqrt[Pi]*Erf[v/vs]*Sign[v]/2
        ];
      Viscous+Coulomb+Stribeck
]
SetAttributes[JointFrictionPotential,Listable]
JointFrictionPotential[v_,Fv_,Fc_]:=JointFrictionPotential[v,Fv,Fc,Fc,0];

(*
SimplifyDiracDelta1[expr_]:=
  Module[{A=expr},Print["in DD0"];While[MatchQ[A, x_. + (a_.) (y_)^n_. DiracDelta[y_] c2_./;
        (xx = x;FreeQ[c2, y]&& FreeQ[a, y] && IntegerQ[n] && n > 0)],
     A=xx];A]
*)

SimplifyDiracDelta1[expr_]:=SimplifyDiracDelta2[SimplifyDiracDelta3[expr]]
SetAttributes[SimplifyDiracDelta1,Listable];
SimplifyDiracDelta3[expr_] :=
  Module[{k0,A,B, list, nodd, dd, ddcases, infy, xx,      
     DDFreeQ, simplified=False},
     DDFreeQ[z_] := FreeQ[z, DiracDelta];
     If[Head[expr] =!= Plus,
        k0 = 0;A =expr,
        (* avoid expanding terms FreeQ of DiracDelta *)
        list = Apply[List, expr];
        nodd = Cases[list, z_ /; DDFreeQ[z]];
        dd = Cases[list, z_ /; !DDFreeQ[z]];
        k0 = Apply[Plus, nodd];A =Apply[Plus, dd]
     ];
     ddcases = Cases[A, DiracDelta[___], 2];
     A=Collect[A, ddcases];
     While[MatchQ[A, x_. + (a_.) (y_)^n_. DiracDelta[(b_.)*(y_)*(c_.)] (c2_.)/;
        (xx = x; FreeQ[{a,b,c,c2}, y] &&IntegerQ[n] && n > 0)],
        simplified = True; A=xx];
     While[MatchQ[A, x_. + (a_.) Abs[z_]^n_. DiracDelta[(b_.)*(y_)*(c_.)] (c2_.)/;
        (xx = x; (z==-y||z==y)&&FreeQ[{a,b,c,c2}, y] &&IntegerQ[n] && n > 0)],
        simplified = True; A=xx];
     B=If[simplified,
          A + k0,
          expr];
     ArgumentSimplify[B]
  ];
SetAttributes[SimplifyDiracDelta3,Listable];

ArgumentSimplify[expr_]:=Module[{AbsRule={},SignRule={},pabs,expr1,psign,p2,TempRule},
   pabs=Cases[expr,Abs[_],Infinity];
   p2=Complement[Level[pabs,2],pabs];
   While[p2=!={},
      TempRule=Inner[Rule,Map[Abs,Cases[p2,-p2[[1]]]],
      Map[Abs,-Cases[p2,-p2[[1]]]],List];
      AbsRule=Join[AbsRule,TempRule];
      p2=Complement[Intersection[p2/.TempRule],{p2[[1]]}]
    ];
   expr1=expr/.AbsRule;
   psign=Cases[expr1,Sign[_],Infinity];
   p2=Complement[Level[psign,2],psign];
   While[p2=!={},
      TempRule=Inner[Rule,
      Map[Sign,Cases[p2,-p2[[1]]]],-Map[Sign,-Cases[p2,-p2[[1]]]],List];
      SignRule=Join[SignRule,TempRule];
      p2=Complement[Intersection[p2/.TempRule],{p2[[1]]}]
    ];
   expr1/.SignRule
  ];
SetAttributes[ArgumentSimplify,Listable];

UnitStep2Sign[expr_]:=Module[{expr1,expr2},
   infy = (Head[Power::infy] === $Off);
   Off[Power::infy];
   expr1=expr/.{UnitStep[x_]->(1+Sign[x])/2};
   expr2=Simplify[Expand[ArgumentSimplify[expr1]]];
   If[!infy, On[Power::infy]];
   expr2
];
SetAttributes[UnitStep2Sign,Listable];

SimplifyDiracDelta2[expr_] :=
  Module[{k0, expand, list, nodd, dd, ddcases, infy, xx, simplified = False},
     If[Head[expr] =!= Plus,
        k0 = 0; expand = Expand[expr],
        (* avoid expanding terms FreeQ of DiracDelta *)
        list = Apply[List, expr];
        nodd = Cases[list, z_ /; DDFreeQ[z]];
        dd = Cases[list, z_ /; !DDFreeQ[z]];
        k0 = Apply[Plus, nodd]; expand = Expand[Apply[Plus, dd]]
     ];
     ddcases = Cases[expand, DiracDelta[___], 2];
     expand = Collect[expand, ddcases];
     ddcases = Cases[expand, Derivative[n_][DiracDelta][___], 2];
     expand = Collect[expand, ddcases];
     While[MatchQ[expand, x_. + (a_.) (y_)^n_. DiracDelta[___, y_] /;
        (xx = x; FreeQ[a, y] && IntegerQ[n] && n > 0)],
        simplified = True; expand = xx];
     While[MatchQ[expand, x_. + (a_.) (y_)^m_Integer?Positive *
        Derivative[n__Integer?NonNegative][DiracDelta][x1___, y_, x2___] /;
        (xx = x; FreeQ[a, y] && Length[{n}] == Length[{x1, y, x2}] &&
         m > {n}[[ Length[{x1}] + 1 ]])],
        simplified = True; expand = xx];        
     infy = (Head[Power::infy] === $Off);
     Off[Power::infy];
     While[MatchQ[expand, x_. + a_ DiracDelta[c1_. y_ + c0_.] /;
               (xx = x;
                Head[y]===Symbol && !NumericQ[y] && FreeQ[{c0, c1}, y] &&
                !FreeQ[a, y] && USDDFreeQ[a] &&
                (0 === Simplify[a /. y :> -c0/c1]) &&
                Integrate[a DiracDelta[c1 y + c0], y] === 0)],
        simplified = True; expand = xx];
     While[MatchQ[expand, x_. +
               a_ Derivative[n_Integer?Positive][DiracDelta][c1_. y_ + c0_.] /;
               (xx = x;  FreeQ[{c0, c1}, y] &&
                !FreeQ[a, y] && USDDFreeQ[a] &&
                (0 === Simplify[a /. y :> -c0/c1]) &&
                Integrate[a Derivative[n][DiracDelta][c1 y + c0], y] === 0)],
        simplified = True; expand = xx];
     If[!infy, On[Power::infy]];
     While[MatchQ[expand, x_. + a0_. DiracDelta[c1_. y_ + c0_.] +
              a1_. Derivative[n_Integer?Positive][DiracDelta][c1_. y_ + c0_.] /;
               (xx = x;  FreeQ[{c0, c1}, y] &&
                (!FreeQ[a0, y] || !FreeQ[a1, y]) &&
                 USDDFreeQ[{a0, a1}] &&
                Integrate[a0 DiracDelta[c1 y + c0] + a1 *
                 Derivative[n][DiracDelta][c1 y + c0], y] === 0)],
           simplified = True; expand = xx];
        (* repeat without US FreeQ *)
        While[MatchQ[expand, x_. + a_ DiracDelta[c1_. y_ + c0_.] /;
               (xx = x;
                Head[y]===Symbol && !NumericQ[y] && FreeQ[{c0, c1}, y] &&
                !FreeQ[a, y] && DDFreeQ[a] &&
                (0 === Simplify[a /. y :> -c0/c1]) &&
                Integrate[a DiracDelta[c1 y + c0], y] === 0)],
        simplified = True; expand = xx];
     While[MatchQ[expand, x_. +
               a_ Derivative[n_Integer?Positive][DiracDelta][c1_. y_ + c0_.] /;
               (xx = x;  FreeQ[{c0, c1}, y] &&
                !FreeQ[a, y] && DDFreeQ[a] &&
                (0 === Simplify[a /. y :> -c0/c1]) &&
                Integrate[a Derivative[n][DiracDelta][c1 y + c0], y] === 0)],
        simplified = True; expand = xx];
     If[!infy, On[Power::infy]];
     While[MatchQ[expand, x_. + a0_. DiracDelta[c1_. y_ + c0_.] +
              a1_. Derivative[n_Integer?Positive][DiracDelta][c1_. y_ + c0_.] /;
               (xx = x;  FreeQ[{c0, c1}, y] &&
                (!FreeQ[a0, y] || !FreeQ[a1, y]) &&
                 USDDFreeQ[a1] && DDFreeQ[a0] &&
                Integrate[a0 DiracDelta[c1 y + c0] + a1 *
                 Derivative[n][DiracDelta][c1 y + c0], y] === 0)],
           simplified = True; expand = xx];
     (* end repeat *)
     If[simplified,
         expand + k0,
         expr]
  ] /; (Position[expr, DiracDelta[___, a_. y_Symbol + b_.] /;
                 FreeQ[{a, b}, y]] =!= {}) ||
       (Position[expr,
         Derivative[n__Integer?NonNegative][DiracDelta][___,
            a_. y_Symbol + b_.] /; FreeQ[{a, b}, y]] =!= {});
SimplifyDiracDelta2[expr_] := expr;
SetAttributes[SimplifyDiracDelta2,Listable];


SimplifyUnitStep2[expr_]:=Module[{newexpr=expr,z1,z2,m},
    While[MatchQ[newexpr,
       (a_. + b_. (-c1_. UnitStep[-(y_)] c2_.+c1_. UnitStep[(y_)] c2_.)^n_. c3_.)/;
        (z1=a + b (c1  c2)^n c3;
         z2=a + b c1^n(- UnitStep[-(y)] +UnitStep[(y)] )c2^n c3;
         m=n;
         FreeQ[y,UnitStep] && IntegerQ[n] && n >1)],
       If[EvenQ[m],newexpr=z1,newexpr=z2]];         
    SimplifyUnitStep[newexpr]
  ]

(***************************** specialized FreeQ ****************************)

RuleFreeQ[expr_] := FreeQ[expr, Rule];
USFreeQ[expr_] := FreeQ[expr, UnitStep];
DDFreeQ[expr_] := FreeQ[expr, DiracDelta];
USDDFreeQ[expr_] := USFreeQ[expr] && DDFreeQ[expr];
LimitFreeQ[expr_] := FreeQ[expr, Limit];


End[]

EndPackage[ ]

(* ********************************************************************** *)
(* ********************************************************************** *)
If[!spell1, On[General::spell1]];
If[!spell, On[General::spell]];

