#!/usr/bin/python

# tz_pid.py
#       --copyright--                   Copyright 2010 (C) Tranzoa, Co. All rights reserved.    Warranty: You're free and on your own here. This code is not necessarily up-to-date or of public quality.
#       --url--                         http://www.tranzoa.net/tzpython/
#       --email--                       pycode is the name to send to. tranzoa.com is the place to send to.
#       --bodstamps--
#       January 6, 2010         bar     borrow logic from some C code
#       May 27, 2012            bar     doxygen namespace
#       February 16, 2013       bar     comments
#       --eodstamps--
##      \file
#       \namespace              tzpython.tz_pid
#
#
#       PID controller.
#
#
#       TODO:
#           When the error is added to the isum, the error can be normalized for time by, say, dividing it by the seconds since the previous sample.
#               As would the terr-prev_err value.
#
#           See elsewhere for twiddle logic for auto-setting the mult's.
#
#


class   a_pid(object) :

    def __init__(me, pmult = 0.0, imult = 0.0, dmult = 0.0, target = 0.0, min_err = None, max_err = None, min_output = None, max_output = None, min_isum = None, max_isum = None) :
        me.pmult        = float(pmult or 0.0)
        me.imult        = float(imult or 0.0)
        me.dmult        = float(dmult or 0.0)

        me.target       = float(target or 0.0)

        if  min_err    == None :
            min_err     = -10000000000000000.0
        if  max_err    == None :
            max_err     =  10000000000000000.0
        if  min_output == None :
            min_output  = -10000000000000000.0
        if  max_output == None :
            max_output  =  10000000000000000.0
        if  min_isum   == None :
            min_isum    = -10000000000000000.0
        if  max_isum   == None :
            max_isum    =  10000000000000000.0
        me.min_err      = min_err
        me.max_err      = max_err
        me.min_output   = min_output
        me.max_output   = max_output
        me.min_isum     = min_isum
        me.max_isum     = max_isum

        me.isum         = 0.0
        me.prev_err     = 0.0

        me.output       = 0.0


    def set_target(me, target = None) :
        ov              = me.target
        if  target     != None :
            me.target   = target

        return(ov)


    def get_next(me, sample, target = None) :
        me.set_target(target)

        if  sample         != None :

            sample          = float(sample or 0.0)

            me.desired      = me.target
            terr            = me.target - sample
            mxe             = False
            if  terr        < me.min_err :
                terr        = me.min_err
                me.desired  = sample   + terr
                mxe         = True
            if  terr        > me.max_err :
                terr        = me.max_err
                me.desired  = sample   + terr
                mxe         = True

            p               = me.pmult * terr

            if  not me.imult :
                me.isum     = 0.0
            i               = me.isum + (me.imult * terr)
            i               = max(i, me.min_isum)
            i               = min(i, me.max_isum)

            d               = me.dmult * (terr - me.prev_err)
            me.prev_err     = terr

            o               = (p + i + d) / 3.0

            if  (mxe or (o >= me.min_output)) and (o <= me.max_output) :
                me.isum     = i

            o               = max(o, me.min_output)
            o               = min(o, me.max_output)

            me.output       = o

        return(me.output, me.desired)


    #   a_pid


#
#
#       Test code
#
#
if __name__ == '__main__' :

    import  random


    ok_mc   = 5
    add_v   = 1.3

    target  = 40
    t       = target


    def     set_ap() :
        ap  = add_v
        if  random.random() >= 0.5 :
            ap  = -ap
        return(ap)


    me      = a_pid(5.8, 1.00, 2.0, target = t, min_output = 0, max_output = 85, min_err = -5, max_err = 5)

    v       = 0.0
    mc      = 0
    dc      = 0
    ap      = set_ap()
    for i in xrange(10000) :
        ( p, d )    = me.get_next(v, t)
        if  dc     >= 5 :
            v      += (p / 10.0 * (       dc  / 100.0))
            v      += (p / 10.0 * ((100 - dc) / 100.0))
            v      += ap
            if  ap  > -add_v :
                ap -= 0.1                                   # don't keep piling on external power. Move toward friction.
            pass
        v           = min(v,  t * 3)
        v           = max(v,  0)
        dc         += 1
        dc          = min(dc, 50)


        iv          = int(round(v))
        ind         = min(t, iv)


        if  ind    == t :
            c       = '*'
            oc      = '+'
        if  ind    == iv :
            c       = '-'
            if  ind == t :
                c   = '|'
            oc      = '*'
        ts          = ""
        if  iv     != t :
            ts      = (" " * (max(t, iv) - ind - 1)) + oc
        ps          = ("%4u" % (i + 1)) + (" " * ind) + c + ts
        print       ps, " " * max(0, 100 - len(ps)), round(t), round(v), round(p), round(d)


        if  t      != iv :
            mc      = 0
        else        :
            mc     += 1
            if  mc >= ok_mc :
                mc  = 0
                dc  = 0
                t   = int(round(target + ((random.random() - 0.5) * target / 2)))
                ap  = set_ap()
            pass
        pass

    print


#
#
# eof