# micropython
# Execution ticks: 37533 ms: 38 drift:0 cycles:1000
# 11 bit PIO or 0x0800
# Execution ticks: 7349408 ms: 7349 speedup: 16001.02 microseconds per cycle: 38.27571 cycles: 12
# Execution ticks: 65367 ms: 66
# 37.533 / 125 = 4691 window size??
"""
These are the 4 pio for each coil connection. In a 4-step cycle the coils are energized in the following sequence:

| Step | A+ | A- | B+ | B- |
|------|-----|-----|-----|-----|
| 1  | ON | OFF | OFF | ON |
| 2  | OFF | ON | OFF | ON |
| 3  | OFF | ON | ON | OFF |
| 4  | ON | OFF | ON | OFF |

In this table:
- **ON** means the end of the coil is being driven or energized to create a magnetic field.
- **OFF** means the end of the coil is not being energized.

For the PIO each one is a sinusoidal wave with a 90 degree phase shift, Sin - Cos - -Sin - -Cos

| Coil | Wave |
|------|------|
| A+  | Sin |
| A-  | Cos |
| B+  | -Sin |
| B-  | -Cos |

PWM is centered around 50% duty cycle. And the values are shared between the 4 pio. 

| High 16 bits | Low 16 bits |
|--------------|-------------|
| Sin     | Cos     |

The high watermark is set to 0x0400 and is counted down to 0x0400. Because two instructions are use per each cycle, the high watermark is half of the window size. 
Effectively the duty cycle is still 0x0400 half per the instructions and doubled per going from X to -X.
The negative values for B+ and B- are achieved by decreasing the input instead of the high watermark.

"""

"""
This is adjusted for the TB6612FNG

| IN1 | IN2 | PWM | STBY | OUT1 | OUT2 | Mode     |
|-----|-----|-----|------|------|------|---------------|
| H  | H  | H/L | H  | L  | L  | Short brake  |
| L  | H  | H  | H  | L  | H  | CCW      |
| H  | L  | H  | H  | H  | L  | CW      |
| L  | H  | L  | H  | L  | L  | Short brake  |
| L  | L  | H  | H  | OFF | OFF | Stop     |
| H/L | H/L | H/L | L  | OFF | OFF | Standby    |

PWMA, AIN1, AIN2 will be used for one winding.

PWMA will recive the duty cycle, which will be the absoulte value of sine.
AIN1, and AIN2 will change every half signal


PWMA, AIN1, AIN2 will be offset a quarter 


"""

import utime
import math
import time
import micropython
from machine import Pin
import rp2

# Constants

SIGNAL_SAMPLES_PER_MS = 42

MS_PER_SAMPLE = 1 / SIGNAL_SAMPLES_PER_MS
SAMPLES_PER_PERIOD = 27
SINE_ANGLE_RES = 1024
COSINE_OFFSET = int(SINE_ANGLE_RES) // int(4)
SINE_TABLE = [int(0)] * (SINE_ANGLE_RES + COSINE_OFFSET)
SINE_RAD_PER_SAMPLE = 2 * math.pi / SINE_ANGLE_RES
# testing slower frequencies too: 125_000_000, 250_000, 62_500, 31_250, 15_625, 7_812, 3_906, 1_953
# MACHINE_FREQ = 125_000_000#7_812
MACHINE_FREQ = 40_000_000
SINE_SCALE = int(500)  # lowered scale to ensure PIO is in range for now
SINE_ZERO = int(
    252
)  # to invert the signal  SINE_ZERO - (sig - SINE_ZERO) = 2 * SINE_ZERO - sig
SINE_INVERT = int(2 * SINE_ZERO)

# scaled

SINE_RAD_PER_SAMPLE_SCALED = int(SINE_RAD_PER_SAMPLE * 32768)

# Default values

RPM = 600
ELECTRIC_ROTATION_PER_TURN = 100
TURNS_PER_SECOND = RPM / 60
ELECTRIC_ROTATIONS_PER_SECOND = ELECTRIC_ROTATION_PER_TURN * TURNS_PER_SECOND
ELECTRICAL_RADIANS_PER_MS = 2 * 3.14159265359 * ELECTRIC_ROTATIONS_PER_SECOND / 1000
RADIANS_PER_SAMPLE = ELECTRICAL_RADIANS_PER_MS * MS_PER_SAMPLE
RADIANS_PER_SAMPLE_PERIOD = RADIANS_PER_SAMPLE * SAMPLES_PER_PERIOD
TEST_CYCLES = int(1000)
SINE_ANGLE_RES_PER_SAMPLE = (
    ELECTRIC_ROTATIONS_PER_SECOND / 1000 * MS_PER_SAMPLE * SINE_ANGLE_RES
)

# State

machine_a_pwm = object()
machine_b_pwm = object()

last_rotation_radians = 0
last_timestamp = 0
action = 1  # START = 0, TEST = 1, CONtINUE = 2, STOP = 3
START, TEST, CONTINUE, STOP = 0, 1, 2, 3
print_debug = False
delta_samples = 0

print("Hello, Pi Pico Micro Python")
time.sleep(0.1)


def load1024Sine():
    for i in range(SINE_ANGLE_RES + COSINE_OFFSET):
        SINE_TABLE[i] = (
            int(SINE_SCALE * abs(math.sin(SINE_RAD_PER_SAMPLE * i)))
            + int(SINE_ZERO)
            - int(SINE_SCALE // 2)
        )


def load1024Sine1():
    for i in range(SINE_ANGLE_RES + COSINE_OFFSET):
        SINE_TABLE[i] = (
            int(SINE_SCALE * max(math.sin(SINE_RAD_PER_SAMPLE * i), 0))
            + int(SINE_ZERO)
            - int(SINE_SCALE // 2)
        )


def load1024Sine2():
    for i in range(SINE_ANGLE_RES + COSINE_OFFSET):
        SINE_TABLE[i] = int(SINE_SCALE * math.sin(SINE_RAD_PER_SAMPLE * i) // 2) + int(
            SINE_ZERO
        )


GPIO_PWMA = Pin(6, Pin.OUT)
GPIO_AIN1 = Pin(2, Pin.OUT)
GPIO_AIN2 = Pin(3, Pin.OUT)

GPIO_PWMB = Pin(7, Pin.OUT)
GPIO_BIN1 = Pin(4, Pin.OUT)
GPIO_BIN2 = Pin(5, Pin.OUT)


@micropython.viper
def main_loop():
    global last_timestamp, delta_samples
    last_timestamp = time.ticks_us()
    sample_index_inc = int(round(SINE_ANGLE_RES_PER_SAMPLE)) % int(SINE_ANGLE_RES)
    # last_sample_number = int( int(last_timestamp) * float(SIGNAL_SAMPLES_PER_MS)) // 1000
    sample_pre_index = int(
        0
    )  # int( int(last_sample_number) * float(SINE_ANGLE_RES_PER_SAMPLE) ) % int(SINE_ANGLE_RES)
    sample_index = int(
        0
    )  # int( int(last_sample_number) * float(SINE_ANGLE_RES_PER_SAMPLE) ) % int(SINE_ANGLE_RES)
    middle_point = int(SINE_ANGLE_RES) // 2
    quarter_point = int(SINE_ANGLE_RES) // 4
    second_quarter_point = int(SINE_ANGLE_RES) * 3 // 4
    # while True:
    for i in range(int(TEST_CYCLES)):
        signal_a_pwm = int(SINE_TABLE[sample_index])
        signal_b_pwm = int(
            SINE_TABLE[int(sample_index) + int(COSINE_OFFSET)]
        )  # 90 degree phase shift, the table is extended to avoid a modulo operation

        # print("Signal PWMA: ", signal_a_pwm)
        # print("Signal PWMB: ", signal_b_pwm)
        # print("Sample index: ", sample_index)
        fifo_size = int(
            machine_a_pwm.tx_fifo()
        )  # Here we have time to do the calculations
        if int(fifo_size) >= 40:
            now = int(time.ticks_us())
            delta = time.ticks_diff(now, last_timestamp)
            last_timestamp = now
            delta_samples = (
                int(int(delta_samples) + int(delta) * int(SIGNAL_SAMPLES_PER_MS))
                // 1000
            )
            if int(delta_samples) > 5:
                delta_samples = 0
                print("Samples: ", sample_index)
                print("Signal PWM: ", signal_a_pwm)
                print("Signal PWM: ", signal_b_pwm)

        machine_a_pwm.put(signal_a_pwm)
        machine_b_pwm.put(signal_b_pwm)
        old_sample_index = sample_index  # on lower sample_index toggle AIN1 and AIN2, on crossing middle point toggle BIN1 and BIN2
        sample_index = int(int(sample_index_inc) + int(sample_index)) % int(
            SINE_ANGLE_RES
        )
        # sample_pre_index += 1
        # sample_index = int(sample_pre_index // 16) % int(SINE_ANGLE_RES)
        if int(old_sample_index) > int(sample_index):
            # print("A Sample index: ", sample_index, " old sample index: ", old_sample_index, " middle point: ", middle_point)
            GPIO_BIN2.value(1)
            GPIO_BIN1.value(0)
            GPIO_AIN1.value(1)
            GPIO_AIN2.value(0)

        if int(old_sample_index) < int(quarter_point) and int(sample_index) >= int(
            quarter_point
        ):
            # print("C Sample index: ", sample_index, " old sample index: ", old_sample_index, " quarter_point: ", quarter_point)
            GPIO_BIN1.value(1)
            GPIO_BIN2.value(0)
            GPIO_AIN1.value(1)
            GPIO_AIN2.value(0)

        if int(old_sample_index) < int(middle_point) and int(sample_index) >= int(
            middle_point
        ):
            # print("B Sample index: ", sample_index, " old sample index: ", old_sample_index, " middle point: ", middle_point)
            GPIO_BIN1.value(1)
            GPIO_BIN2.value(0)
            GPIO_AIN2.value(1)
            GPIO_AIN1.value(0)

        if int(old_sample_index) < int(second_quarter_point) and int(
            sample_index
        ) >= int(second_quarter_point):
            # print("D Sample index: ", sample_index, " old sample index: ", old_sample_index, " second_quarter_point: ", second_quarter_point)
            GPIO_BIN2.value(1)
            GPIO_BIN1.value(0)
            GPIO_AIN2.value(1)
            GPIO_AIN1.value(0)


@rp2.asm_pio(
    set_init=rp2.PIO.OUT_LOW,
    out_shiftdir=rp2.PIO.SHIFT_LEFT,
    in_shiftdir=rp2.PIO.SHIFT_LEFT,
    autopull=True,
)
def custom_program():
    # Initialize ISR
    set(pins, 0b1)  # Set a different pin to high for error
    set(x, 0b1)  # Set x to 1
    mov(isr, x)  # Copy x into the ISR
    in_(null, 9)  # Shift in half the window

    label("start_cycle")
    mov(x, isr)
    out(y, 32)
    jmp(not_y, "start_cycle")

    label("lead_cycle")
    jmp(x_dec, "test_lead_cycle")

    label("test_lead_cycle")
    jmp(x_not_y, "lead_cycle")

    label("rise")
    set(pins, 1)
    mov(y, invert(y))

    label("lag_cycle")
    jmp(x_dec, "test_lag_cycle")

    label("test_lag_cycle")
    jmp(x_not_y, "lag_cycle")

    label("fall")
    set(pins, 0)
    mov(y, invert(isr))

    label("end_cycle")
    jmp(x_dec, "test_end_cycle")

    label("test_end_cycle")
    jmp(x_not_y, "end_cycle")

    jmp("start_cycle")  # Loop back to the start


def init_pio():
    global machine_a_pwm, machine_b_pwm
    machine_a_pwm = rp2.StateMachine(
        1, custom_program, freq=MACHINE_FREQ, set_base=GPIO_PWMA
    )
    machine_b_pwm = rp2.StateMachine(
        3, custom_program, freq=MACHINE_FREQ, set_base=GPIO_PWMB
    )
    machine_a_pwm.active(1)
    machine_b_pwm.active(1)
    GPIO_AIN1.value(0)
    GPIO_AIN2.value(1)
    GPIO_BIN1.value(1)
    GPIO_BIN2.value(0)


def stop_pio():
    global machine_a_pwm, machine_b_pwm
    machine_a_pwm.active(0)
    machine_b_pwm.active(0)


def main():
    load1024Sine()
    print("Sine table loaded")
    init_pio()
    print(
        "PIO initialized", int(round(SINE_ANGLE_RES_PER_SAMPLE)) % int(SINE_ANGLE_RES)
    )
    time.sleep(1)
    start_ticks = time.ticks_us()
    start_millis = time.ticks_ms()
    main_loop()
    end_ticks = time.ticks_us()
    end_millis = time.ticks_ms()
    stop_pio()
    if MACHINE_FREQ == 7_812:
        speedup = 125000000 / 7812
        cycles = TEST_CYCLES * speedup
        us_per_cycle = time.ticks_diff(end_ticks, start_ticks) / cycles
        print(
            "Execution ticks: ", time.ticks_diff(end_ticks, start_ticks),
            " ms: ", time.ticks_diff(end_millis, start_millis),
            " speedup: ", speedup,
            " microseconds per cycle: ", us_per_cycle,
            " cycles:", TEST_CYCLES,
        )
    else:
        print(
            "Execution ticks: ", time.ticks_diff(end_ticks, start_ticks),
            " ms: ", time.ticks_diff(end_millis, start_millis),
            " drift: ", delta_samples,
        )


main()
print("Done!!")