##############################################################################
### PID Controller Library and Example
##############################################################################
###   Like all of the scripts in my folder here, this file contains
###   functions you might want to include into your own scripts for
###   actual use and a demo in the 'main' function that you can just
###   run to see how it works.
###
###   This file includes a simple and generic PID controller that we use in
###   many of the other examples when we want to smoothly control one value
###   based on our measurement of another.  The PID class docstring explains
###   the basics of using it in your project.   The demo code below that
###   shows how to use the PID controller to hold a vertical velocity with
###   variation of engine thrust.

###   From https://github.com/krpc/krpc-library/blob/master/Art_Whaleys_KRPC_Demos/pid.py
##############################################################################

import time
import krpc


class PID(object):
    '''
    Generic PID Controller Class
    Based on the PID recipe at :

    http://code.activestate.com/recipes/577231-discrete-pid-controller/

    and the code and discussions in the blog at:

    http://brettbeauregard.com/blog/2011/04/
    improving-the-beginners-pid-introduction/

    An instance is created with the format
    your_pid=PID(P=.0001, I=0.00001, D=0.000001)

    Finding the right values for those three gain numbers is called 'tuning' and
    that's beyond the scope of this doc string!

    Use your_pid.setpoint(X) to set the target output value of the controller.

    Regularly call your_pid.update(Y), passing it the input data that the
    controller should respond to.
    output_data = your_pid.update(input_data)

    '''

    def __init__(self, P=1.0, I=0.1, D=0.01):
        self.Kp = P  # P controls reaction to the instantaneous error
        self.Ki = I  # I controls reaction to the history of error
        self.Kd = D  # D prevents overshoot by considering rate of change
        self.P = 0.0
        self.I = 0.0
        self.D = 0.0
        self.SetPoint = 0.0  # Target value for controller
        self.ClampI = 1.0  # clamps i_term to prevent 'windup.'
        self.LastTime = time.time()
        self.LastMeasure = 0.0

    def update(self, measure):
        now = time.time()
        change_in_time = now - self.LastTime
        if not change_in_time:
            change_in_time = 1.0  # avoid potential divide by zero if PID just created.

        error = self.SetPoint - measure
        self.P = error
        self.I += error
        self.I = self.clamp_i(self.I)  # clamp to prevent windup lag
        self.D = (measure - self.LastMeasure) / (change_in_time)

        self.LastMeasure = measure  # store data for next update
        self.lastTime = now

        return (self.Kp * self.P) + (self.Ki * self.I) - (self.Kd * self.D)

    def clamp_i(self, i):
        if i > self.ClampI:
            return self.ClampI
        elif i < -self.ClampI:
            return -self.ClampI
        else:
            return i

    def setpoint(self, value):
        self.SetPoint = value
        self.I = 0.0


##############################################################################
## Demo Code Below This Line!
##############################################################################

Target_Velocity = 5  # The value we're trying to limit ourselves to


##############################################################################
## Main  -- only run when we execute this file directly.
##          ignored when we import the PID into other files!
##############################################################################
def main():
    # Setup KRPC
    conn = krpc.connect()
    sc = conn.space_center
    v = sc.active_vessel
    telem = v.flight(v.orbit.body.reference_frame)

    #  Create PID controller.
    p = PID(P=.25, I=0.025, D=0.0025)
    p.ClampI = 20
    p.setpoint(Target_Velocity)

    #  starting with locked SAS and throttle at full
    v.control.sas = True
    v.control.throttle = 1.0
    while not v.thrust:  # stage if we just launched a new rocket
        v.control.activate_next_stage()

    #  Loop Forever, or until you get the point of this example and stop it.
    while True:
        the_pids_output = p.update(telem.vertical_speed)
        v.control.throttle = the_pids_output
        print('Vertical V:{:03.2f}   PID returns:{:03.2f}   Throttle:{:03.2f}'
              .format(telem.vertical_speed,
                      the_pids_output,
                      v.control.throttle))
        time.sleep(.1)


if __name__ == '__main__':
    main()
    print('--')