#!/usr/bin/env python3

import sys
import os
from subprocess import *


#
# read some stuff out of the INI file
#

# servo period in nanoseconds
servo_period_ns = float(Popen(
    [
        "inivar",
        "-var",
        "SERVO_PERIOD",
        "-sec",
        "EMCMOT",
        "-ini",
        "%s/motion-test.ini" % os.getenv("TEST_DIR")
    ],
    stdout=PIPE
).communicate()[0])

cycles_per_second = 1000000000.0 / servo_period_ns

# max acceleration in inches per second^2
max_acceleration_ips2 = float(Popen(
    [
        "inivar",
        "-var",
        "MAX_ACCELERATION",
        "-sec",
        "AXIS_X",
        "-ini",
        "%s/motion-test.ini" % os.getenv("TEST_DIR")
    ],
    stdout=PIPE
).communicate()[0])

# max acceleration in inches per second per servo-thread cycle
# the factor of half at the end is because emc2 reserves half the acceleration for blending
max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) * 0.5

# max velocity in inches per second
max_velocity_ips = float(Popen(
    [
        "inivar",
        "-var",
        "MAX_VELOCITY",
        "-sec",
        "AXIS_X",
        "-ini",
        "%s/motion-test.ini" % os.getenv("TEST_DIR")
    ],
    stdout=PIPE
).communicate()[0])

max_velocity_ipc = max_velocity_ips / cycles_per_second


# read the samples file and turn it into an array, where index i is an
# array of the floats found on line i of the samples file
samples_filename = sys.argv[1] + ".halsamples"
with open(samples_filename) as f:
    lines = f.readlines()

samples = []
for line in lines:
    s = [ float(x) for x in line.split() ]
    samples.append(s)


#
# here comes the test proper
#


# verify that X starts at 0.000000
if samples[0][1] != 0.000000:
    print("error: sample 0: X starts at %.6f, not at 0.000000" % samples[0][1])
    sys.exit(1)

#print("line 0: X starts at 0.000000")


# find where X starts moving (X is the second column, column 1)
for i in range(0, len(samples) - 1):
    if samples[i][1] != samples[i+1][1]:
        break

if len(samples) == i+2:
    print("error: no acceleration detected, did linuxcnc take too long to start moving?")
    sys.exit(1)

#print("line %d: accel phase starts" % i)


# now i is the sample before it starts moving
# verify accel does not exceed maxaccel
# verify vel does not exceed maxvel
# verify accel phase stops when vel == maxvel
highest_seen_accel_ipc2 = 0
old_accel_ipc2 = 0
old_v_ipc = 0
for i in range(i, len(samples) - 1):

    new_v_ipc = samples[i+1][1] - samples[i][1]
    accel_ipc2 = new_v_ipc - old_v_ipc

    highest_seen_accel_ipc2 = max(accel_ipc2, highest_seen_accel_ipc2)

    #print("line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second)))

    # i hate floating point
    if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001: 
        print("error: line %d: detected accel constraint violation!" % i)
        print("error: detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second)))
        print("error: max accel %.6f i/s^2)" % max_acceleration_ips2)
        sys.exit(1)

    if (new_v_ipc - max_velocity_ipc) > 0.0000001:
        print("error: line %d: detected vel constraint violation!" % i)
        print("error: detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second))
        print("error: max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips))
        sys.exit(1)

    if accel_ipc2 == 0:
        print("line %d: accel phase over" % i)
        break

    old_v_ipc = new_v_ipc

# verify highest seen accel is very close to max accel
if abs(max_acceleration_ipspc - (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:
    print("error: accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second)))
    print("error: max accel is %.6f i/s^2" % max_acceleration_ips2)
    sys.exit(1)

#print("    accel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2)

#print("line %d: entering cruise phase, vel=%.6f i/s, max_accel = %.6f i/s^2" % (i, new_v_ipc * cycles_per_second, highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second))


# now i is the first sample of the cruise phase
# wait for vel to drop again
for i in range(i, len(samples) - 1):
    new_v_ipc = samples[i+1][1] - samples[i][1]

    if abs(new_v_ipc - old_v_ipc) > 0.0000001:
        # decel phase starting
        break

    old_v_ipc = new_v_ipc

#print("line %d: decel phase starting, old_v=%.6f i/s, new_v=%.6f i/s" % (i, old_v_ipc * cycles_per_second, new_v_ipc * cycles_per_second))

# now i is the sample before it starts decel
# verify accel does not exceed maxaccel
# verify vel does not exceed maxvel
# verify decel phase stops when vel == 0
highest_seen_accel_ipc2 = 0
old_accel_ipc2 = 0
for i in range(i, len(samples) - 1):

    new_v_ipc = samples[i+1][1] - samples[i][1]
    accel_ipc2 = new_v_ipc - old_v_ipc

    highest_seen_accel_ipc2 = min(accel_ipc2, highest_seen_accel_ipc2)

    #print("line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second)))

    # i hate floating point
    if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001: 
        print("error: line %d: detected accel constraint violation!" % i)
        print("error: detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, accel_ipc2 * cycles_per_second * cycles_per_second))
        print("error: max accel %.6f i/s^2)" % max_acceleration_ips2)
        sys.exit(1)

    if (new_v_ipc - max_velocity_ipc) > 0.0000001:
        print("error: line %d: detected vel constraint violation!" % i)
        print("error: detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second))
        print("error: max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips))
        sys.exit(1)

    if new_v_ipc < -0.0000001:
        print("error: line %d: detected vel undershoot!" % i)
        print("error: detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second))
        sys.exit(1)

    if new_v_ipc == 0:
#        print("line %d: decel phase over" % i)
        break

    old_v_ipc = new_v_ipc

# verify highest seen accel is very close to max accel
if abs(max_acceleration_ipspc + (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:
    print("error: accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second)))
    print("error: max accel is %.6f i/s^2" % max_acceleration_ips2)
    sys.exit(1)

#print("    decel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2)


# verify X stopped at 1.000000
if samples[i][1] != 1.000000:
    print("error: line %d: X stopped at %.6f, not at 1.000000!" % (i, samples[i][1]))
    sys.exit(1)

#print("    X reached target of 1.0000")


# verify X doesn't move from now on
for i in range(i, len(samples) - 1):
    if samples[i][1] != samples[i+1][1]:
        print("error: line %d: X moved!" % i)
        sys.exit(1)


#print("line %d: X did not move after reaching target" % i)

sys.exit(0)


