# Modified by Grasshopper June 17, 2022, 12:38PM CDT
"""
Grasshopper code fix 6/9/22

FF0000 = Red
770000 = Rust Red
FFFF00 = Yellow
00FF00 = Green
00FFFF = Light Blue
0000FF = Blue
FF00FF = Lavender
000000 = Black
FFFFFF = White
0F0F0F = Leopard Skin
"""
from stepper import motor_move  # import motor_move from stepper.py

# from home import go_move  # import go_home from home.py
import board  # core module in circuit python on ESP32
import displayio  # core module in circuit python on ESP32
import terminalio  # core module in circuit python on ESP32
import math  # core module in circuit python on ESP32
from adafruit_display_text import label
from adafruit_st7789 import ST7789  # TFT driver for ESP32 TFT
from rainbowio import colorwheel  # colorwheel for neopixel
from adafruit_seesaw import seesaw, neopixel, rotaryio, digitalio
import time

# Turn on I2C power pin
i2c_power = digitalio.DigitalIO(board.TFT_I2C_POWER, 0x36)

# set up I2C
i2c = board.I2C()
# set up SPI
spi = board.SPI()

displayio.release_displays()

# tft_cs = board.D5
tft_cs = board.TFT_CS
# tft_dc = board.D6
tft_dc = board.TFT_DC
display_bus = displayio.FourWire(spi, command=tft_dc, chip_select=tft_cs)

# display_bus = displayio.ST7789
# for some reason rowstart = 40 will push the text to the left
# rowstart = 30 will leave a gap on the left of the screen
display = ST7789(
    display_bus,
    rotation=270,
    width=240,
    height=135,
    rowstart=40,
    colstart=53,
)
seesaw = seesaw.Seesaw(board.I2C(), addr=0x37)

seesaw.pin_mode(24, seesaw.INPUT_PULLUP)
button = digitalio.DigitalIO(seesaw, 24)
button_held = False

encoder = rotaryio.IncrementalEncoder(seesaw)
last_position = None
text = "No Data"
degree = 0
sec = 0
lev = 0
cur_pos = 0
second = 0
decimal_second = second / 60

# ready Neopixel
pixel = neopixel.NeoPixel(seesaw, 6, 1)
pixel.brightness = 0.2
color = 0  # start at red
# red=0 yellow=25 green=64 yellow=25 blue=150 lavender=240 red=255

# Make the display context
splash = displayio.Group()
display.show(splash)

color_bitmap = displayio.Bitmap(128, 32, 1)
color_palette = displayio.Palette(1)
color_palette[0] = 0x000000  # White

# Draw a smaller inner rectangle
inner_bitmap = displayio.Bitmap(128, 32, 1)
inner_palette = displayio.Palette(1)
inner_palette[0] = 0x000000  # Black
inner_sprite = displayio.TileGrid(inner_bitmap, pixel_shader=inner_palette, x=0, y=0)
splash.append(inner_sprite)

# Draw a label
text = " MicroArc\nRotary"
text_area = label.Label(terminalio.FONT, text=text, color=0xFFFF00, x=0, y=15, scale=4)
splash.append(text_area)
time.sleep(0.5)

second_position = 0  # second as determined by the position input

# function to move the fence
# dist is the distance of the move
# x = degrees
# y = seconds converted to afraction of a degree
# decimal_second = seconds converted to afraction of a degree
# Z = current position
def move(x, y, z):
    dist = 0
    if z > (x + decimal_second):
        dist = -(z - (x + decimal_second))
        z = z - dist

    elif z < (x + decimal_second):
        dist = (x + decimal_second) - z
        z = z + dist

    text_area.text = "Move\n{}".format(dist)
    print("At line 116: Dist for move: " + str(dist) + " degrees")
    motor_move(dist)
    time.sleep(0.2)


# function to obtain the second of an degree (if any) to be added to the move
def setsec(y):
    last_position = -y
    position = -y
    button_held2 = False
    # Convert . back to int position #
    encoder.position = -y
    text_area.text = "+Seconds:\n {}".format(int(math.fmod(float(y), 60.0)))

    p = True
    while p:

        position = -encoder.position

        if position < 0:
            position = 0
            encoder.position = 0

        if position != last_position:
            last_position = position
            text_area.text = "+Seconds:\n {}".format(
                int(math.fmod(float(position), 60.0))
            )

        if not button.value:
            button_held2 = True

        if button.value and button_held2:
            p = False
            button_held2 = False
    # decimal_second = second/60 so at some point we
    # need to convert seconds to fraction of a degree.
    y = position  # /60
    print("At line 154: Y= " + str(y))
    return y


# function to obtain the whole degrees to be moved minus any second
def setdegree(x):
    last_position = -x
    position = -x
    button_held = False
    encoder.position = -x
    text_area.text = "Degrees:\n {}".format(int(math.fmod(float(x), 360.0)))

    p = True
    while p:

        position = -encoder.position

        if position < 0:
            position = 0
            encoder.position = 0
        if position != last_position:
            last_position = position
            text_area.text = "Degrees:\n {}".format(
                int(math.fmod(float(position), 360.0))
            )
            x = int(math.fmod(float(position), 360.0))

        if not button.value:
            button_held = True

        if button.value and button_held:
            p = False
            button_held = False

    return x


# function to level Rotary Table
def level(lev, mode):
    last_position = -lev
    position = -lev
    button_held = False
    encoder.position = -lev
    text_area.text = " Level:\n {}".format(int(math.fmod(float(lev), 360.0)))

    while mode == 0:

        position = -encoder.position

        if position != last_position:
            position = last_position - position
            encoder.position = 0
            lev = int(math.fmod(float(position), 360))
            print("lev=", lev, "Mode: ", mode)
            move(lev, 0, 0)

        if not button.value:
            button_held = True
            # this seems redundant (around line 226)
            mode = 1
            print(button.value, "Mode=", mode)

        if button.value and button_held:
            mode = 1
            button_held = False

    return mode


# function to enter the number of teeth
def NumTeeth(a):
    """
    last_position = -a
    position = -a
    button_held = False
    """
    encoder.position = -a
    if a <= 0:
        a = 0
        print(str(encoder.position))
    text_area.text = "Enter # of\nteeth: {}".format(int(math.fmod(float(a), 360.0)))
    if a == 0:
        a = 1
    return a


pixel.fill(colorwheel(0))
# set up Manual Move
# While true:
mode = 0

# Mode = 0 will level the axis if needed.
while mode == 0:
    lev = -encoder.position
    level(lev, mode)
    mode = 1
    time.sleep(0.5)

# Mode = 1 will get number of teeth
while mode == 1:
    a = -encoder.position
    teeth = NumTeeth(a)
    print("At line 256: Mode:", mode, " a=", a, " Teeth=", teeth)

    if not button.value:
        button_held = True
        # this seems redundant (around line 226)
        mode = 2
        last_position = 0
        a = 0
        print("At line 264: Button value=", button.value, "  Mode=", mode)
time.sleep(0.5)

# Mode = 2 will move the wheel one tooth at a time so you may cut each in turn
while mode == 2:
    print("At line 269: Mode:", mode, "a=", a)
    text_area.scale = 3
    text_area.text = "Use MPG\nto rotate to\nnext postion:"
    last_position = -a
    position = -a
    button_held = False
    # encoder.position = -a
    print("At line 276: Mode:", mode, "Teeth =", teeth)
    arc = 360 / teeth
    # line correction
    degree = int(arc)
    decimal_second = (360 % teeth) / teeth
    print(
        "At line 282: Mode:",
        mode,
        " Position=",
        position,
        " Last position=",
        last_position,
        " Arc=",
        arc,
        "degree=",
        degree,
        "decimal=",
        decimal_second,
        "Button value=",
        button.value,
    )

    p = True
    while p:

        position = abs(int(math.fmod(float(encoder.position), teeth)))
        a =  position - last_position

        if position != last_position:
            print("\nAt line 305 Mode=", mode, "Postion after fmod=", position)
        if position < 0:
            position = -position
            encoder.position = 0
        if position != last_position:
            print((
                "At lin 311: Mode:",
                mode,
                " Degree=",
                degree,
                " Decimal second=",
                decimal_second,
                " Position=",
                position,
                " Last position=",
                last_position,
                " a=",
                a,
            ))
            last_position = position
            move((a * degree), decimal_second, 0)
            text_area.scale = 4
            text_area.text = "Tooth:\n  {}".format(
                int(math.fmod(float(position), teeth))
            )
        if not button.value:
            button_held = True
        if button.value and button_held:
            p = False
            button_held = False

    mode = 3

# Mode = 3 lets you turn the wheel to a given angle -
while mode == 3:
    cur_pos = degree + decimal_second
    print("At line 341: Mode:", mode, "Cur_pos: " + str(cur_pos))
    pixel.fill(colorwheel(95))
    degree = setdegree(degree)
    print("At line 344: Degrees: " + str(degree))
    pixel.fill(colorwheel(42))
    second = setsec(second)
    decimal_second = second / 60
    print("At line 348: Seconds: " + str(second) + " sec: " + str(decimal_second))
    pixel.fill(colorwheel(0))
    move(degree, decimal_second, cur_pos)
    print(
        "At line 352: Move Degree: "
        + str(degree)
        + " plus: "
        + str(decimal_second)
        + " and pre-move postion is: "
        + str(cur_pos)
    )

    """
    I need an exit strategy for this loop
    if not button.value:
        button_held = True
        # this seems redundant (around line 226)
        mode = 0
        print(button.value, "Mode=", mode)
    """
