import GCode_Interpreterdc

class Controller:
    def __init__(self):
        pass

    def __call__(self, gtm, *args, **kwargs):
        assert isinstance(gtm)
        return None

# We control position, but the output of the controller is a set speed.
# Therefore, we have proportional, differential and second differential terms.
class PIDController(Controller):
    def __init__(self, p, i, d):
        err_x, err_y, err_z = 0., 0., 0.
        derr_x, derr_y, derr_z = 0., 0., 0.
        d2err_x, d2err_y, d2err_z = 0., 0., 0.

                self.p, self.i, self.d = p,i,d

    def __call__(self, gtm, *args, **kwargs):
        err_x_new, err_y_new, err_z_new = gtm.delta_steps
        derr_x_new, derr_y_new, derr_z_new =
            err_x_new - err_x, err_y_new - err_y, err_z_new - err_z
        d2err_x_new, d2err_y_new, d2err_z_new =
            derr_x_new - derr_x, derr_y_new - derr_y, derr_z_new - derr_z

        err_x,err_y,err_z = err_x_new, err_y_new, err_z_new
        derr_x,derr_y,derr_z = derr_x_new, derr_y_new, derr_z_new
        d2err_x,d2err_y,d2err_z = d2err_x_new,d2err_y_new,d2err_z_new

        speed_cmd_x = self.p * derr_x + self.i * err_x + self.d * d2err_x
        speed_cmd_y = self.p * derr_y + self.i * err_y + self.d * d2err_y
        speed_cmd_z = self.p * derr_z + self.i * err_z + self.d * d2err_z

        # It is the responsability of the coefficients to make sure this results in the desired feedrate
        gtm.x_throttle = gtm.target_feedrate/abs(speed_cmd_x)
        gtm.x_direction = 1 if (speed_cmd_x > 0) else 0

        gtm.y_throttle = gtm.target_feedrate/abs(speed_cmd_y)
        gtm.y_direction = 1 if (speed_cmd_y > 0) else 0

        gtm.z_throttle = gtm.target_feedrate/abs(speed_cmd_z)
        gtm.z_direction = 1 if (speed_cmd_z > 0) else 0

        if(gtm.x_throttle > 65536):
            gtm.x_throttle = 0
        if(gtm.y_throttle > 65536):
            gtm.y_throttle = 0
        if(gtm.z_throttle > 65536):
            gtm.z_throttle = 0