import pylab
import math

class Massa:

    def __init__(self, _M, _b):
        self.M = _M
        self.b = _b
        # variabili di stato, p = posizione, v = velocita'
        self.p = 0
        self.v = 0

    def evaluate(self, _input, dt):
        self.p = self.p + delta_t * self.v
        self.v = (1 - self.b * dt/self.M) * self.v + dt / self.M * _input

    def get_position(self):
        return self.p

    def get_speed(self):
        return self.v



class PISat:

    def __init__(self, kp, ki, sat):
        self.kp = kp
        self.ki = ki
        self.saturation = sat
        self.integral = 0
        self.saturation_flag = False

    def evaluate(self, target, current, delta_t):
        error = target - current
        if not(self.saturation_flag):
            self.integral = self.integral + error * delta_t
        output = self.kp * error + self.ki * self.integral
        if output > self.saturation:
            output = self.saturation
            self.saturation_flag = True
        elif output < -self.saturation:
            output = -self.saturation
            self.saturation_flag = True
        else:
            self.saturation_flag = False
        return output



class VirtualRobotPositionController:

    def __init__(self, vmax, accel, decel):
        self.speed_controller = PISat(400, 1000, 200)
        self.position_controller = PISat(2, 0, vmax)
        self.vmax = vmax
        self.accel = accel
        self.decel = decel
        self.target_pos = 0
        self.target_speed = 0
        self.decel_distance = self.vmax * self.vmax / (2 * self.decel)
        self.decel_time = self.vmax / self.decel
        self.current_time = 0
        self.virtual_robot_speed = 0
        self.virtual_robot_pos = 0


    def compute_virtual_robot_pos(self, delta_t):
        if (self.target_pos - self.virtual_robot_pos) < self.decel_distance:
            # fase di decelerazione
            current_accel = -self.decel
        else:
            # fase di accelerazione o moto a vel costante
            current_accel = self.accel

        self.virtual_robot_speed = self.virtual_robot_speed + current_accel * delta_t

        if self.virtual_robot_speed >= self.vmax:
            self.virtual_robot_speed = self.vmax
            current_accel = 0

        if self.virtual_robot_speed <= 0:
            self.virtual_robot_speed = 0
            current_accel = 0

        self.virtual_robot_pos = self.virtual_robot_pos + self.virtual_robot_speed * delta_t + \
          0.5 * current_accel * delta_t * delta_t


    def set_target(self, t):
        self.target_pos = t


    def evaluate(self, current_pos, current_speed, delta_t):

        self.compute_virtual_robot_pos(delta_t)

        self.target_speed = self.position_controller.evaluate(self.virtual_robot_pos,
                                                              current_pos, delta_t)

        f = self.speed_controller.evaluate(self.target_speed,
					 current_speed, delta_t)

        return f


delta_t = 0.001 # 1 ms

robot = Massa(6.0, 25.0)
# vmax = 2 m/s
# accel = 0.5 m/s2
# deccel = 0.5 m/s2
v_robot_controller = VirtualRobotPositionController(2, 0.8, 0.4)

t = 0.0
vettore_pos = [ ]
vettore_virtual_pos = [ ]
vettore_vel = [ ]
vettore_target_vel = [ ]
vettore_virtual_vel = [ ]
vettore_tempi = [ ]
vettore_f = [ ]

v_robot_controller.set_target(20)  # 6 m

while t < 20:

    current_pos = robot.get_position()
    current_speed = robot.get_speed()

    f = v_robot_controller.evaluate(current_pos, current_speed, delta_t)

    robot.evaluate(f, delta_t)

    vettore_vel.append(current_speed)
    vettore_target_vel.append(v_robot_controller.target_speed)
    vettore_virtual_vel.append(v_robot_controller.virtual_robot_speed)

    vettore_pos.append(current_pos)
    vettore_virtual_pos.append(v_robot_controller.virtual_robot_pos)

    vettore_tempi.append(t)

    t = t + delta_t


pylab.figure(1)
pylab.plot(vettore_tempi, vettore_vel, 'r-+', label='vel')
pylab.plot(vettore_tempi, vettore_virtual_vel, 'g-+', label='virtual vel')
pylab.plot(vettore_tempi, vettore_target_vel, 'b-+', label='target vel')
pylab.xlabel('time')
pylab.legend()

pylab.figure(2)
pylab.plot(vettore_tempi, vettore_pos, 'r-+', label='position')
pylab.plot(vettore_tempi, vettore_virtual_pos, 'g-+', label='virtual position')
pylab.xlabel('time')
pylab.legend()


pylab.show()