(***)

BeginPackage["DSPCtrl`FreqResp`","Graphics`Graphics`"]

S::usage=
"S[f] is the Laplace operator which gives complex value 2 Pi f I. f
is the Freqency."

Z::usage=
"Z[f,Ts] is the Z opretor which gives complex value Exp[2 Pi f I
 Ts]. f is the frequency.And Ts is the sampling period."

OpenToClose::usage=
"OpenToClose[expr] translates expr of the openloop transfer function
into closed loop transfer function."

CloseToOpen::usage=
"CloseToOpen[expr] translates expr of the closed loop transfer
function into open loop transfer function."

OpenToERR::usage=
"OpenToERR[expr] translates expr of the open loop transfer function
into error rejection ratio transfer function. 1/(1+expr)."

CloseToERR::usage=
"CloseToERR[expr] translates expr of the closed loop transfer
function into error rejection ratio transfer function. 1-expr."

GainResponse::usage=
"GainResponse[expr,f] gives the gain(dB) of expr of the transfer
function at the frequency f."

PhaseResponse::usage=
"PhaseResponse[expr,f] gives phase(degree) of expr of the transfer
function at the frequency f."

GainCurve::usage=
"GainCurve[expr,{f,fmin,fmax},gmin,gmax,di,opts]"

PlotGains::usage=
"PlotGains[expr,{f,fmin,fmax},gmin,gmax,di,opts] generates a
gain Bode plot of the transfr function expr[f].expr[f] is defined
as a polynomial of S[f](laplace form) of Z[f Ts](Z form).List
expression is available for the expr.fmin and fmax limit frequency
range.gmin and gmax limit gain range.di indicate Tick interval
for the vertical axis."

PhaseCurve::usage=
"PhaseCurve[expr,{f,fmin,fmax},pmin,pmax,di,opts]"

PlotPhases::usege=
"PlotPhages[expr,{f,fmin,fmax},pmin,pmax,di,opts] generates a
phase Bode plot of transfer function expr[f].expr[f] is defined as a
polynomial of S[f](Laplace form) or Z[f,Ts](Z form).List expression
is available for the expr.fmin and fmax limit frequency range.pmin
and pmax limit phase range.di indicates Tick interval for the
vertical axis."

DrawGainPlane::usage=
"DrawGainPlane[fmin,fmax,gmin,gmax,di] generater frequency-gain
plane for Bode plot.fmin and fmax limit frequency range in Hz.gmin
and gmax limit gain range in dB.di gives the Tick interval of
vertical axis."

DrawPhasePlane::usage=
"DrawPhasePlane[fmin,fmax,pmin,pmax,di] generates frequency-phase
plane for Bode plot.fmin and fmax limit frequency range in Hz.pmin
and pmax limit phase range in degree.di gives Tick interval of
vertical axis."


Begin["`Private`"]

S[f_]:=I 2 Pi f

Z[f_,ts_]:=Exp[S[f] ts]

OpenToClose[expr_]:=expr/(1+expr)

CloseToOpen[expr_]:=expr/(1-expr)

OpenToERR[expr_]:=1/(1+expr)

CloseToERR[expr_]:=1-expr

GainResponse[expr_,f_,fg_]:=N[20.0 Log[10,Abs[expr /. f->fg]]]

PhaseResponse[expr_,f_,fp_]:=N[Arg[expr /. f->fp] 180/Pi]

GainCurve[expr_,{f_,fmin_,fmax_},gmin_,gmax_,di_,opts___]:=
	ParametricPlot[{fg,GainResponse[expr,f,10^fg]},
					{fg,Log[10,fmin],Log[10,fmax]},
	PlotDivision -> 30.,
	PlotRange -> {{Log[10,fmin],Log[10,fmax]},
					{gmin,gmax}},
	Ticks -> {LogScale,Range[N[N[gmin],1],N[N[gmax],1],di]},
	opts
	];

PlotGains[expr_,{f_,fmin_,fmax_},gmin_,gmax_,di_,opts___]:=
	Block[{NoOfExpr,GainCurves,i,GraphOut},
		NoOfExpr:=Length[expr];
		GainCurves:=If[!VectorQ[expr],
			GainCurve[N[expr],{f,fmin,fmax},gmin,gmax,di,
				opts,PlotStyle -> RGBColor[1,0,1]],
			Table[GainCurve[N[expr[[i]]],{f,fmin,fmax},gmin,
				gmax,di,opts,
				PlotStyle -> RGBColor[N[(NoOfExpr-i)/NoOfExpr],
									N[(i-1)/NoOfExpr],1]],
				{i,1,NoOfExpr}]
				];
		GraphOut:=
			Show[{GainCurves,
				DrawGainPlane[fmin,fmax,gmin,gmax,di]},
				AxesOrigin->{Floor[Log[10,fmin]],Automatic},
				AxesLabel->{"Hz","   dB"},
				PlotRange->{{Floor[Log[10,fmin]],
							Ceiling[Log[10,fmax]]},
							{gmin,gmax}},
				PlotLabel->
					"Frequency-Gain response of transfer function(s)."
				];
	Return[GraphOut] ]

PhaseCurve[expr_,{f_,fmin_,fmax_},pmin_,pmax_,di_,opts___]:=
	ParametricPlot[{fp,PhaseResponse[expr,f,10^fp]},
					{fp,Log[10,fmin],Log[10,fmax]},
					PlotDivision->30.,
					PlotRange->{{Log[10,fmin],Log[10,fmax]},
								{pmin,pmax}},
					Ticks->{LogScale,Range[N[N[pmin],1],
					N[N[pmax],1],di]},
					opts
				];

PlotPhases[expr_,{f_,fmin_,fmax_},pmin_,pmax_,di_,opts___]:=
	Block[{NoOfExpr,PhaseCurves,i,GraphOut},
		NoOfExpr:=Length[expr];
		PhaseCurves:=If[!VectorQ[expr],
			PhaseCurve[N[expr],{f,fmin,fmax},pmin,pmax,di,
				opts,PlotStyle->RGBColor[1,0,1]],
			Table[PhaseCurve[N[expr[[i]]],{f,fmin,fmax},pmin,pmax,
				di,opts,
				PlotStyle->RGBColor[N[(NoOfExpr-i)/NoOfExpr],
									N[(i-1)/NoOfExpr],1]],
				{i,1,NoOfExpr}]
				];
		GraphOut:=
			Show[{PhaseCurves,
				DrawPhasePlane[fmin,fmax,pmin,pmax,di]},
				AxesOrigin->{Floor[Log[10,fmin]],Automatic},
				AxesLabel->{"Hz","   Degree"},
				PlotRange->{{Floor[Log[10,fmin]],
							Ceiling[Log[10,fmax]]},
							{pmin,pmax}},
				PlotLabel->
					"Frequency-Phase response of transfer function(s)"
					] ;
			Return[GraphOut] ]

DrawGainPlane[fmin_,fmax_,gmin_,gmax_,di_]:=
	Graphics[{Thickness[0.001],
			{Release[Table[Line[
				{{N[Log[10,10^Floor[Log[10,N[fmin]]]]],k},	
				{N[Log[10,10^Ceiling[Log[10,N[fmax]]]]],k}}],
				{k,N[N[gmin],1],N[N[gmax],1],di}]]},
			{Release[Table[Line[
				{{N[Log[10,n 10^m]],N[N[gmin],1]},
				{N[Log[10,n 10^m]],N[N[gmax],1]}}],
				{n,1,10,1},
				{m,Floor[Log[10,fmin]],
					Ceiling[Log[10,fmax]]-1,1}]]}} ]

DrawPhasePlane[fmin_,fmax_,pmin_,pmax_,di_]:=
	Graphics[{Thickness[0.001],
			{Release[Table[Line[
				{{N[Log[10,10^Floor[Log[10,N[fmin]]]]],k},	
				{N[Log[10,10^Ceiling[Log[10,N[fmax]]]]],k}}],
				{k,N[N[pmin],2],N[N[pmax],2],di}]]},
			{Release[Table[Line[
				{{N[Log[10,n 10^m]],N[N[pmin],1]},
				{N[Log[10,n 10^m]],N[N[pmax],1]}}],
				{n,1,10,1},
				{m,Floor[Log[10,fmin]],
					Ceiling[Log[10,fmax]]-1,1}]]}} ] 

End[]
EndPackage[]