def calcX(t, Px, Vx, k):
    return Px + Vx * (1 - np.exp(-k * t)) / k

def calcY(t, Py, Vy, k):
    return Py + Vy * (1 - np.exp(-k * t)) / k

def calcZ(t, Pz, Vz, k, g):
    return Pz - (g * t + (np.exp(-k * t) - 1) * (g / k + Vz)) / k

def calcVelX(t, Vx, k):
    return Vx * np.exp(-k * t)

def calcVelY(t, Vy, k):
    return Vy * np.exp(-k * t)

def calcVelZ(t, Vz, k, g):
    return -(g - np.exp(-k * t) * (g + k * Vz)) / k

def calcT(Xref, Px, Vx, k):
    return -np.log(1 - k * (Xref - Px) / Vx) / k

class Point:
    def __init__(self, t, x, y, z):
        self.t = t
        self.x = x
        self.y = y
        self.z = z

def distance(point1, point2):
    return math.sqrt((point2.x - point1.x) ** 2 + (point2.y - point1.y) ** 2 + (point2.z - point1.z) ** 2 )

class BallMovement():

    def __init__(self):
        self.pointBuffer = []
        self.fly = False
        self.minFlySpeed = 3
        self.timeLim = 0.1
        self.polynom = False
        self.p = 1
        self.maxPredErr = 0.5
        self.newTraectory = []
        self.polyCount = 20
        self.kx = None
        self.ky = None
        self.kz = None
        self.count = 0

    def checkBeginVectorPoint(self, vector, point):
        v1 = np.array([
            (point.x - vector[1].x) / (point.t - vector[1].t),
            (point.y - vector[1].y) / (point.t - vector[1].t),
            (point.z - vector[1].z) / (point.t - vector[1].t),
        ])
        v2 = np.array([
            (vector[1].x - vector[0].x) / (vector[1].t - vector[0].t),
            (vector[1].y - vector[0].y) / (vector[1].t - vector[0].t),
            (vector[1].z - vector[0].z) / (vector[1].t - vector[0].t),
        ])
        if ((np.linalg.norm(v1) > self.minFlySpeed)
            and (np.linalg.norm(v1 - v2) < self.minFlySpeed * 0.1)
            and ((point.t - vector[0].t) < self.timeLim)
            and ((point.z - vector[0].z) > 0)):
            return True
        else:
            return False


    def checkBeginVector(self, vector):
        v1 = np.array([
            (vector[2].x - vector[1].x) / (vector[2].t - vector[1].t),
            (vector[2].y - vector[1].y) / (vector[2].t - vector[1].t),
            (vector[2].z - vector[1].z) / (vector[2].t - vector[1].t),
        ])
        v2 = np.array([
            (vector[1].x - vector[0].x) / (vector[1].t - vector[0].t),
            (vector[1].y - vector[0].y) / (vector[1].t - vector[0].t),
            (vector[1].z - vector[0].z) / (vector[1].t - vector[0].t),
        ])
        if ((np.linalg.norm(v1) > self.minFlySpeed)
            and (np.linalg.norm(v1 - v2) < self.minFlySpeed * 0.1)
            and ((vector[2].t - vector[0].t) < self.timeLim)
            and ((vector[2].z - vector[0].z) > 0)):
            return True
        else:
            return False

    def pushPoint(self, point):
        if (len(self.pointBuffer) < 2 and not self.fly):
            self.pointBuffer.append(point)
        elif (len(self.pointBuffer) == 2 and not self.fly):
            if (self.checkBeginVectorPoint(self.pointBuffer, point)):
                self.fly = True
            else:
                self.pointBuffer.pop(0)
                self.pointBuffer.append(point)
        if (self.fly):
            if (len(self.pointBuffer) < 3):
                self.pointBuffer.append(point)
            elif self.polynom:
                ex = np.polyval(self.kx, point.t) - point.x
                ey = np.polyval(self.ky, point.t) - point.y
                ez = np.polyval(self.kz, point.t) - point.z
                E = np.linalg.norm(np.array([ex, ey, ez]))
                if (E > self.maxPredErr):
                    self.newTraectory.append(point)
                else:
                    if (len(self.pointBuffer) == self.polyCount):
                        self.pointBuffer.pop(0)
                    self.pointBuffer.append(point)
                    self.newTraectory = []
                if (len(self.newTraectory) == 3):
                    self.pointBuffer = self.newTraectory
                    self.fly = False
                    self.polynom = False
                    self.newTraectory = []
                    if (self.checkBeginVector(self.pointBuffer)):
                        self.fly = True
                    else:
                        self.pointBuffer.pop(0)
            x = np.array([p.x for p in self.pointBuffer])
            y = np.array([p.y for p in self.pointBuffer])
            z = np.array([p.z for p in self.pointBuffer])
            t = np.array([p.t for p in self.pointBuffer])
            if (len(self.pointBuffer) < 5):
                self.p = 1
            if (len(self.pointBuffer) > 5):
                self.p = 2
            p = self.p
            self.kx = np.polyfit(t, x, p)
            self.ky = np.polyfit(t, y, p)
            self.kz = np.polyfit(t, z, p)
            dkx = np.polyder(self.kx)
            dky = np.polyder(self.ky)
            dkz = np.polyder(self.kz)
            self.polynom = True
            T = (self.pointBuffer[len(self.pointBuffer) - 1].t - self.pointBuffer[0].t) / 2
            PxT = np.polyval(self.kx,T)
            PyT = np.polyval(self.ky,T)
            PzT = np.polyval(self.kz,T)
            VxT = np.polyval(dkx, T)
            VyT = np.polyval(dky, T)
            VzT = np.polyval(dkz, T)
            if (len(self.pointBuffer) == self.polyCount):
                t = calcT(1, PxT, VxT, 0.9)
                X = calcX(t, PxT, VxT, 0.9)
                Y = calcY(t, PyT, VyT, 0.9)
                Z = calcZ(t, PzT, VzT, 0.9, 9.8)
                velX = calcVelX(t, VxT, 0.9)
                velY = calcVelY(t, VyT, 0.9)
                velZ = calcVelZ(t, VzT, 0.9, 9.8)
#                 print(t)
                print("X - {}".format(X))
                print("Y - {}".format(Y))
                print("Z - {}".format(Z))
                print(velX)
                print(velY)
                print(velZ)
                self.count += 1
#                 print(self.count)
                return Z, t
#             for i in range(1, len(self.pointBuffer)):
#                 if (distance(self.pointBuffer[i - 1], self.pointBuffer[i])):