import math
import matplotlib.pyplot as plt

from utils.defines import *
from utils.generator import *
from matplotlib.ticker import (MultipleLocator, AutoMinorLocator)

def main():
    st = Stepper(1.8, 32)
    print(f"Step = {st.Step}; Steps = {st.Steps}")

    wv = QuadWave(64)
    # plt.plot(wv.Sine, label="sin")
    # plt.plot(wv.Cosine, label="cos")
    # plt.legend()
    # plt.show()

    gen = Generator(st, 0.001, 1, 1)

    curves = []
    curves.append(gen.createSmoothStep(math.pi)) # 180 deg 
    curves.append(gen.createSmoothStep(math.pi*1/6)) # 60 deg 
    curves.append(gen.createBuildUpStep(math.pi)) # 180 deg 
    curves.append(gen.createRapidStartSwingInEnd(math.pi)) # 180 deg 
    curves.append(gen.createJumpInTarget(math.pi)) # 180 deg 

    fig, axes = plt.subplots(len(curves), 1, layout='constrained')

    for (i,ax) in enumerate(axes):
        crv = curves[i]
        ax.plot(crv["p"], label="ϕ")
        ax.plot(crv["w"], label="ω", linestyle="--")

        dax = ax.twinx()  # instantiate a second Axes that shares the same x-axis
        dax.step(range(len(crv["o"])), crv["o"], label="o", linewidth=.5)
        print(f"EndPos = {crv["pE"]}; (Delta = {math.pi - crv["pE"]}) in {crv["T"] * 1000} steps")

    tMax = max(map(lambda x : x["T"], curves))
    tEnd = math.ceil(tMax)*1000

    for ax in axes:
        ax.yaxis.set_ticks([0, math.pi/3, math.pi*2/3, math.pi],["Picto0", "Picto1", "Picto2", "Picto3"])
        ax.xaxis.set_ticks(range(0,tEnd+1000,1000), range(0,math.ceil(tMax)+1,1))
        ax.xaxis.set_ticks(range(0,tEnd+1000,100), minor = True)
        ax.yaxis.grid(linestyle='dotted')
        ax.set_xlabel('time (s)')
        ax. set_ylabel('angle')
        # ax.legend()

    plt.show()

    
if __name__ == '__main__':
    main()