
import numpy as np
import matplotlib.pyplot as plt
import matplotlib2tikz.save as tikz_save


def pytag(x, y):
    	return np.power(np.power(x, 2) + np.power(y, 2), 1 / 2)


def accumulate_s(t, x, y):
	s = [0]
	for i in range(1, len(t)):
		s.append(s[i - 1] + pytag(np.polyval(x, t[i]) - np.polyval(x, t[i - 1]), np.polyval(y, t[i]) - np.polyval(y, t[i - 1])))
	return np.polyfit(t, s, 10)


circ_x = [
	-5.87E+01,
	-5.89E+01,
	-6.00E+01,
	-5.99E+01,
	-5.98E+01,
	-5.96E+01,
	-5.95E+01,
	-5.93E+01,
	-5.92E+01,
	-5.89E+01,
	-5.87E+01,
	-5.85E+01,
	-5.81E+01,
	-5.78E+01,
	-5.75E+01,
	-5.72E+01,
	-5.67E+01,
	-5.61E+01,
	-5.55E+01,
	-5.48E+01,
	-5.41E+01,
	-5.34E+01,
	-5.26E+01,
	-5.19E+01,
	-5.09E+01,
	-4.99E+01,
	-4.88E+01,
	-4.78E+01,
	-4.66E+01,
	-4.54E+01,
	-4.42E+01,
	-4.29E+01,
	-4.15E+01,
	-4.01E+01,
	-3.87E+01,
	-3.71E+01,
	-3.55E+01,
	-3.39E+01,
	-3.21E+01,
	-3.05E+01,
	-2.87E+01,
	-2.69E+01,
	-2.50E+01,
	-2.31E+01,
	-2.12E+01,
	-1.92E+01,
	-1.72E+01,
	-1.51E+01,
	-1.29E+01,
	-1.08E+01,
	-8.61E+00,
	-6.37E+00,
	-4.06E+00,
	-1.68E+00,
	6.33E-01,
	3.06E+00,
	5.44E+00,
	7.87E+00,
	1.03E+01,
	1.27E+01,
	1.52E+01,
	1.76E+01,
	2.01E+01,
	2.25E+01,
	2.50E+01,
	2.75E+01,
	2.99E+01,
	3.24E+01,
	3.48E+01,
	3.73E+01,
	3.98E+01,
	4.22E+01,
	4.46E+01,
	4.71E+01,
	4.96E+01,
	5.20E+01,
	5.44E+01,
	5.69E+01
]
circ_x = np.divide(circ_x, 100)
circ_y = [
	3.12E+01,
	3.08E+01,
	3.00E+01,
	2.96E+01,
	2.89E+01,
	2.84E+01,
	2.77E+01,
	2.70E+01,
	2.60E+01,
	2.51E+01,
	2.41E+01,
	2.30E+01,
	2.20E+01,
	2.06E+01,
	1.94E+01,
	1.80E+01,
	1.66E+01,
	1.52E+01,
	1.37E+01,
	1.23E+01,
	1.09E+01,
	9.39E+00,
	7.88E+00,
	6.45E+00,
	5.01E+00,
	3.49E+00,
	1.96E+00,
	5.89E-01,
	-8.11E-01,
	-2.29E+00,
	-3.64E+00,
	-5.02E+00,
	-6.33E+00,
	-7.60E+00,
	-8.86E+00,
	-1.01E+01,
	-1.14E+01,
	-1.26E+01,
	-1.37E+01,
	-1.50E+01,
	-1.62E+01,
	-1.73E+01,
	-1.85E+01,
	-1.96E+01,
	-2.07E+01,
	-2.19E+01,
	-2.30E+01,
	-2.41E+01,
	-2.51E+01,
	-2.61E+01,
	-2.70E+01,
	-2.78E+01,
	-2.86E+01,
	-2.93E+01,
	-2.99E+01,
	-3.04E+01,
	-3.09E+01,
	-3.13E+01,
	-3.17E+01,
	-3.21E+01,
	-3.24E+01,
	-3.28E+01,
	-3.30E+01,
	-3.32E+01,
	-3.34E+01,
	-3.36E+01,
	-3.37E+01,
	-3.37E+01,
	-3.38E+01,
	-3.38E+01,
	-3.40E+01,
	-3.38E+01,
	-3.40E+01,
	-3.41E+01,
	-3.40E+01,
	-3.42E+01,
	-3.42E+01,
	-3.44E+01
]
circ_y = np.divide(circ_y, 100)


circ_t = np.divide([i for i in range(len(circ_x))], 100)
circ_xpoly = np.polyfit(circ_t, circ_x, 10)
circ_ypoly = np.polyfit(circ_t, circ_y, 10)
circ_spoly = accumulate_s(circ_t, circ_xpoly, circ_ypoly)


circup_x = [
	-3.72E+01,
	-3.67E+01,
	-3.65E+01,
	-3.61E+01,
	-3.59E+01,
	-3.56E+01,
	-3.52E+01,
	-3.49E+01,
	-3.45E+01,
	-3.41E+01,
	-3.36E+01,
	-3.32E+01,
	-3.28E+01,
	-3.26E+01,
	-3.22E+01,
	-3.17E+01,
	-3.14E+01,
	-3.09E+01,
	-3.04E+01,
	-3.00E+01,
	-2.96E+01,
	-2.92E+01,
	-2.88E+01,
	-2.83E+01,
	-2.77E+01,
	-2.73E+01,
	-2.67E+01,
	-2.61E+01,
	-2.56E+01,
	-2.52E+01,
	-2.47E+01,
	-2.41E+01,
	-2.36E+01,
	-2.33E+01,
	-2.29E+01,
	-2.23E+01,
	-2.18E+01,
	-2.13E+01,
	-2.08E+01,
	-2.03E+01,
	-1.96E+01,
	-1.90E+01,
	-1.83E+01,
	-1.78E+01,
	-1.74E+01,
	-1.68E+01,
	-1.62E+01,
	-1.55E+01,
	-1.48E+01,
	-1.40E+01,
	-1.33E+01,
	-1.27E+01,
	-1.21E+01,
	-1.17E+01,
	-1.11E+01,
	-1.03E+01,
	-9.94E+00,
	-9.19E+00,
	-8.64E+00,
	-8.01E+00,
	-6.63E+00,
	-5.85E+00,
	-5.08E+00,
	-4.40E+00,
	-3.92E+00,
	-3.16E+00,
	-2.14E+00,
	-1.39E+00,
	-6.40E-01,
	1.44E-01,
	9.43E-01,
	1.38E+00,
	2.13E+00,
	2.76E+00,
	3.51E+00,
	4.39E+00,
	6.05E+00,
	6.76E+00,
	7.95E+00,
	8.22E+00,
	9.80E+00,
	1.08E+01,
	1.17E+01,
	1.16E+01,
	1.26E+01,
	1.35E+01,
	1.45E+01,
	1.54E+01,
	1.63E+01,
	1.74E+01,
	1.96E+01,
	2.07E+01,
	2.18E+01,
	2.20E+01,
	2.31E+01,
	2.43E+01,
	2.54E+01,
	2.75E+01,
	2.78E+01,
	2.88E+01,
	3.13E+01,
	3.25E+01,
	3.37E+01,
	3.42E+01,
	3.62E+01,
	3.77E+01,
	3.90E+01,
	4.03E+01,
	4.12E+01,
	4.25E+01,
	4.45E+01,
	4.62E+01,
	4.73E+01,
	4.89E+01,
	5.01E+01,
	5.24E+01,
	5.37E+01,
	5.52E+01,
	5.66E+01,
	5.87E+01,
	6.01E+01,
	6.16E+01,
	6.34E+01,
	6.51E+01
]
circup_x = np.divide(circup_x, 100)
circup_y = [
	2.69E+01,
	2.68E+01,
	2.68E+01,
	2.67E+01,
	2.67E+01,
	2.66E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.66E+01,
	2.66E+01,
	2.66E+01,
	2.66E+01,
	2.65E+01,
	2.65E+01,
	2.64E+01,
	2.64E+01,
	2.64E+01,
	2.63E+01,
	2.62E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.60E+01,
	2.59E+01,
	2.59E+01,
	2.57E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.55E+01,
	2.54E+01,
	2.54E+01,
	2.54E+01,
	2.55E+01,
	2.54E+01,
	2.53E+01,
	2.52E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.48E+01,
	2.48E+01,
	2.46E+01,
	2.45E+01,
	2.44E+01,
	2.43E+01,
	2.41E+01,
	2.40E+01,
	2.38E+01,
	2.38E+01,
	2.38E+01,
	2.37E+01,
	2.36E+01,
	2.33E+01,
	2.31E+01,
	2.31E+01,
	2.30E+01,
	2.29E+01,
	2.28E+01,
	2.26E+01,
	2.24E+01,
	2.24E+01,
	2.22E+01,
	2.19E+01,
	2.17E+01,
	2.15E+01,
	2.13E+01,
	2.12E+01,
	2.07E+01,
	2.03E+01,
	2.02E+01,
	1.97E+01,
	1.90E+01,
	1.91E+01,
	1.86E+01,
	1.84E+01,
	1.81E+01,
	1.75E+01,
	1.70E+01,
	1.65E+01,
	1.66E+01,
	1.58E+01,
	1.51E+01,
	1.45E+01,
	1.39E+01,
	1.35E+01,
	1.29E+01,
	1.17E+01,
	1.12E+01,
	1.06E+01,
	9.91E+00,
	8.71E+00,
	7.71E+00,
	6.83E+00,
	5.69E+00,
	4.75E+00,
	3.62E+00,
	2.36E+00,
	1.32E+00,
	2.23E-01,
	-1.37E+00,
	-2.74E+00,
	-4.21E+00,
	-5.58E+00,
	-6.89E+00,
	-7.95E+00,
	-1.02E+01,
	-1.18E+01,
	-1.35E+01,
	-1.53E+01,
	-1.74E+01,
	-1.93E+01,
	-2.14E+01,
	-2.36E+01
]
circup_y = np.divide(circup_y, 100)


circup_t = np.divide([i for i in range(len(circup_x))], 100)
circup_xpoly = np.polyfit(circup_t, circup_x, 10)
circup_ypoly = np.polyfit(circup_t, circup_y, 10)
circup_spoly = accumulate_s(circup_t, circup_xpoly, circup_ypoly)


circup_xreal = [
	-4.81E+01,
	-4.81E+01,
	-4.81E+01,
	-4.80E+01,
	-4.80E+01,
	-4.80E+01,
	-4.79E+01,
	-4.79E+01,
	-4.78E+01,
	-4.78E+01,
	-4.77E+01,
	-4.76E+01,
	-4.76E+01,
	-4.75E+01,
	-4.74E+01,
	-4.74E+01,
	-4.73E+01,
	-4.72E+01,
	-4.72E+01,
	-4.71E+01,
	-4.70E+01,
	-4.69E+01,
	-4.68E+01,
	-4.67E+01,
	-4.66E+01,
	-4.65E+01,
	-4.64E+01,
	-4.63E+01,
	-4.62E+01,
	-4.61E+01,
	-4.60E+01,
	-4.58E+01,
	-4.57E+01,
	-4.56E+01,
	-4.54E+01,
	-4.53E+01,
	-4.52E+01,
	-4.50E+01,
	-4.49E+01,
	-4.46E+01,
	-4.45E+01,
	-4.42E+01,
	-4.41E+01,
	-4.38E+01,
	-4.36E+01,
	-4.33E+01,
	-4.31E+01,
	-4.28E+01,
	-4.25E+01,
	-4.22E+01,
	-4.20E+01,
	-4.17E+01,
	-4.14E+01,
	-4.11E+01,
	-4.08E+01,
	-4.05E+01,
	-4.02E+01,
	-3.99E+01,
	-3.96E+01,
	-3.92E+01,
	-3.89E+01,
	-3.85E+01,
	-3.82E+01,
	-3.78E+01,
	-3.74E+01,
	-3.72E+01,
	-3.67E+01,
	-3.65E+01,
	-3.61E+01,
	-3.59E+01,
	-3.56E+01,
	-3.52E+01,
	-3.49E+01,
	-3.45E+01,
	-3.41E+01,
	-3.36E+01,
	-3.32E+01,
	-3.28E+01,
	-3.26E+01,
	-3.22E+01,
	-3.17E+01,
	-3.14E+01,
	-3.09E+01,
	-3.04E+01,
	-3.00E+01,
	-2.96E+01,
	-2.92E+01,
	-2.88E+01,
	-2.83E+01,
	-2.77E+01,
	-2.73E+01,
	-2.67E+01,
	-2.61E+01,
	-2.56E+01,
	-2.52E+01,
	-2.47E+01,
	-2.41E+01,
	-2.36E+01,
	-2.33E+01,
	-2.29E+01,
	-2.23E+01,
	-2.18E+01,
	-2.13E+01,
	-2.08E+01,
	-2.03E+01,
	-1.96E+01,
	-1.90E+01,
	-1.83E+01,
	-1.78E+01,
	-1.74E+01,
	-1.68E+01,
	-1.62E+01,
	-1.55E+01,
	-1.48E+01,
	-1.40E+01,
	-1.33E+01,
	-1.27E+01,
	-1.21E+01,
	-1.17E+01,
	-1.11E+01,
	-1.03E+01,
	-9.94E+00,
	-9.19E+00,
	-8.64E+00,
	-8.01E+00,
	-6.63E+00,
	-5.85E+00,
	-5.08E+00,
	-4.40E+00,
	-3.92E+00,
	-3.16E+00,
	-2.14E+00,
	-1.39E+00,
	-6.40E-01,
	1.44E-01,
	9.43E-01,
	1.38E+00,
	2.13E+00,
	2.76E+00,
	3.51E+00,
	4.39E+00,
	6.05E+00,
	6.76E+00,
	7.95E+00,
	8.22E+00,
	9.80E+00,
	1.08E+01,
	1.17E+01,
	1.16E+01,
	1.26E+01,
	1.35E+01,
	1.45E+01,
	1.54E+01,
	1.63E+01,
	1.74E+01,
	1.96E+01,
	2.07E+01,
	2.18E+01,
	2.20E+01,
	2.31E+01,
	2.43E+01,
	2.54E+01,
	2.75E+01,
	2.78E+01,
	2.88E+01,
	3.13E+01,
	3.25E+01,
	3.37E+01,
	3.42E+01,
	3.62E+01,
	3.77E+01,
	3.90E+01,
	4.03E+01,
	4.12E+01,
	4.25E+01,
	4.45E+01,
	4.62E+01,
	4.73E+01,
	4.89E+01,
	5.01E+01,
	5.24E+01,
	5.37E+01,
	5.52E+01,
	5.66E+01,
	5.87E+01,
	6.01E+01,
	6.16E+01,
	6.34E+01,
	6.51E+01
]
circup_xreal = np.divide(circup_xreal, 100)
circup_yreal = [
	2.69E+01,
	2.69E+01,
	2.69E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.68E+01,
	2.69E+01,
	2.69E+01,
	2.70E+01,
	2.70E+01,
	2.71E+01,
	2.71E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.72E+01,
	2.71E+01,
	2.70E+01,
	2.70E+01,
	2.69E+01,
	2.68E+01,
	2.68E+01,
	2.67E+01,
	2.67E+01,
	2.66E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.67E+01,
	2.66E+01,
	2.66E+01,
	2.66E+01,
	2.66E+01,
	2.65E+01,
	2.65E+01,
	2.64E+01,
	2.64E+01,
	2.64E+01,
	2.63E+01,
	2.62E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.61E+01,
	2.60E+01,
	2.59E+01,
	2.59E+01,
	2.57E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.56E+01,
	2.55E+01,
	2.54E+01,
	2.54E+01,
	2.54E+01,
	2.55E+01,
	2.54E+01,
	2.53E+01,
	2.52E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.50E+01,
	2.48E+01,
	2.48E+01,
	2.46E+01,
	2.45E+01,
	2.44E+01,
	2.43E+01,
	2.41E+01,
	2.40E+01,
	2.38E+01,
	2.38E+01,
	2.38E+01,
	2.37E+01,
	2.36E+01,
	2.33E+01,
	2.31E+01,
	2.31E+01,
	2.30E+01,
	2.29E+01,
	2.28E+01,
	2.26E+01,
	2.24E+01,
	2.24E+01,
	2.22E+01,
	2.19E+01,
	2.17E+01,
	2.15E+01,
	2.13E+01,
	2.12E+01,
	2.07E+01,
	2.03E+01,
	2.02E+01,
	1.97E+01,
	1.90E+01,
	1.91E+01,
	1.86E+01,
	1.84E+01,
	1.81E+01,
	1.75E+01,
	1.70E+01,
	1.65E+01,
	1.66E+01,
	1.58E+01,
	1.51E+01,
	1.45E+01,
	1.39E+01,
	1.35E+01,
	1.29E+01,
	1.17E+01,
	1.12E+01,
	1.06E+01,
	9.91E+00,
	8.71E+00,
	7.71E+00,
	6.83E+00,
	5.69E+00,
	4.75E+00,
	3.62E+00,
	2.36E+00,
	1.32E+00,
	2.23E-01,
	-1.37E+00,
	-2.74E+00,
	-4.21E+00,
	-5.58E+00,
	-6.89E+00,
	-7.95E+00,
	-1.02E+01,
	-1.18E+01,
	-1.35E+01,
	-1.53E+01,
	-1.74E+01,
	-1.93E+01,
	-2.14E+01,
	-2.36E+01
]
circup_yreal = np.divide(circup_yreal, 100)

circupreal_t = np.divide([i for i in range(len(circup_xreal))], 100)
circupreal_xpoly = np.polyfit(circupreal_t, circup_xreal, 10)
circupreal_ypoly = np.polyfit(circupreal_t, circup_yreal, 10)
circupreal_spoly = accumulate_s(circupreal_t, circupreal_xpoly, circupreal_ypoly)

def straight():
	return np.polyfit([0, 94.55 / 100], [49.7956 / 100, 0], 1), 0, 94.55 / 100


def circ():
	return np.polyfit(circ_x, circ_y, 10), circ_x[0], circ_x[-1]


def circ_up():
	return np.polyfit(circup_x, circup_y, 10), circup_x[0], circup_x[-1]


def circ_upreal():
	return np.polyfit(circup_xreal, circup_yreal, 10), circup_xreal[0], circup_xreal[-1]


def trvalues(p, x):
	y = np.polyval(p, x)
	dp = np.polyder(p)
	dydx = np.polyval(dp, x)
	ddp = np.polyder(dp)
	d2ydx2 = np.polyval(ddp, x)
	alpha = np.arctan(-dydx)
	R = (1.0 + dydx**2)**1.5 / d2ydx2
	return (y, dydx, d2ydx2, alpha, R)


def dvx(tr, p, x, v_x, g, c):
	ret = g * np.sin(tr[3]) * np.cos(tr[3]) / (1 + c)
	
	return ret


xy = circ_up()

p = xy[0]

x = xy[1]

v_x = 0.35
# v_x = 0.17

b_mass = 1
c = 1.11
# c = 0.792669613
# c = 0
g = 9.81

tr = trvalues(p, x)

alpha = tr[3]

# print(alpha)

h = 0.001

t = 0

s = 0

# xs = np.arange(xy[1], xy[2], 0.0001)

# plt.plot(xs, np.polyval(p, xs), 'k')

# plt.show()

ts = []
xs = []
ss = []
vxs = []
angs = []


while x < xy[2]:
	ts.append(t)
	xs.append(x)
	ss.append(s)
	vxs.append(v_x)
	angs.append(tr[3])
	t += h
	x += h * v_x
	tr = trvalues(p, x)
	s += h * v_x / np.cos(tr[3])
	v_x += h * dvx(tr, p, x, v_x, g, c)


def velocity(vx, ang):
	return np.power(np.power(vx, 2) + np.power(vx * np.tan(ang), 2), 1 / 2)


# print("AccTime:\t", circup_t[-1])
print("Time:\t", t)
print("V:\t", velocity(vxs[-1], angs[-1]))


# plt.plot(circ_t, np.polyval(circ_spoly, circ_t), 'k')						# s(t)	meas
# plt.plot(circ_t, np.polyval(np.polyder(circ_spoly), circ_t), 'k')			# v(t)	meas

plt.plot(circup_t, np.polyval(circup_spoly, circup_t), 'k')				# s(t)	meas
# plt.plot(circup_t, np.polyval(np.polyder(circup_spoly), circup_t), 'k')		# v(t)	meas

# plt.plot(circupreal_t, np.polyval(circupreal_spoly, circupreal_t), 'k')				# s(t)	meas
# plt.plot(circupreal_t, np.polyval(np.polyder(circupreal_spoly), circupreal_t), 'k')		# v(t)	meas

# plt.plot(ts, xs)
# plt.plot(ts, np.polyval(p, xs))
plt.plot(ts, ss, 'Grey')														# s(t)	calc
# plt.plot(ts, vxs, "blue")
# plt.plot(ts, vxs * np.tan(angs), "orange")
# plt.plot(ts, velocity(vxs, angs), 'grey')										# v(t)	calc

plt.xlabel("t, [s]")
# plt.ylabel("v(t), [m/s]")
plt.ylabel("s(t), [m]")

plt.grid(True)
# tikz_save("f2.tikz", figureheight='\\figureheight', figurewidth='\\figurewidth')

# plt.savefig("f3.png")

plt.show()
