#!/usr/bin/python
#################################################################################
### This program was Written by Matthew Toia and is released as public source ###
### on the condition that he recieves credit for his work.                    ###
###                                                                           ###
### If you make additions to this code please email controls@fiftyvolts.com   ###
### and your work my be included in a future release.                         ###
###                                                                           ###
### CONTROL LAW MAKER v0.1                                                    ###
#################################################################################
import sys, re, math

val=0
i=0

## Process command line args ##
while i<len(sys.argv):
    if sys.argv[i]=="-G" and i+1<len(sys.argv):
        G = sys.argv[i+1] #Should probably have some input validation here but, eh. 
        val=val|1
        i=i+1
    elif sys.argv[i]=="-w0" and i+1<len(sys.argv):
        w0 = sys.argv[i+1]
        val=val|2
        i=i+1
    elif sys.argv[i]=="-pm" and i+1<len(sys.argv):
        pm = sys.argv[i+1]
        val=val|4
        i=i+1
    i=i+1 #increment counter
    
## make sure all args have been passed ## 
if val!=7:
    print "Usage: %s -G <plan expression> -w0 <unity gain freq> -pm <phase margine deg.>" % (sys.argv[0])
    sys.exit(1)

print "Finding control law for G=%s w0=%s pm=%s" % (G, w0, pm)

### convert to usable formats ###
Gs=eval(G.replace("s", w0+'j')) # <- sneaky way of saying G(jw0) 
w0 = float(w0)
pm = float(pm)


## Calculate angle of plant ##
angleGs = math.atan(Gs.imag/Gs.real) * 180 / math.pi
if Gs.real<0 and Gs.imag<0:
    angleGs = angleGs-180
elif Gs.real<0:
    angleGs = angleGs+180
    
## Calculate z and p ##
a = math.tan((pm + 90 - angleGs)/4 * math.pi / 180)
z = w0/a
p = w0*a

## Calculate M ##
HGs = ((w0*1j + z)**2)/(w0*1j*(w0*1j+p)**2)*Gs
M = 1 / abs(HGs)

N = "%.2f (s + %.2f)" % (M, z)
D = "s (s + %.2f)" % (p)
length = max(len(N),len(D))
div = "-"*length

padN = int(math.ceil((length-len(N))/2))
padD = int(math.ceil((length-len(D))/2))

print "        %*s" % (padN+len(N), N)
print "HG(s) = %s" % div;
print "        %*s" % (padD+len(D), D)