"""
2-Axis CNC Plotter Firmware
Raspberry Pi Pico (RP2040) — MicroPython
Donanim: 2x TMC2208 (STEP/DIR), 1x Servo GP27
Test edildi: Motor X (GP8/GP9) OK, Motor Y (GP12/GP13) OK, Servo (GP27) OK
"""

from machine import Pin, PWM
from time import ticks_us, ticks_diff, ticks_ms, sleep_us
import sys

# ─── PIN TANIMLARI ────────────────────────────────────────────────────────────
DIR1 = Pin(8, Pin.OUT)  # GP8  → Motor X DIR
STEP1 = Pin(9, Pin.OUT)  # GP9  → Motor X STEP
DIR2 = Pin(12, Pin.OUT)  # GP12 → Motor Y DIR
STEP2 = Pin(13, Pin.OUT)  # GP13 → Motor Y STEP
SERVO_PIN = 27  # GP27 → SERVO_02

# ─── SERVO ────────────────────────────────────────────────────────────────────
# Two supported modes:
# - "ANGLE": standard hobby servo (0..180 degrees)
# - "CR": continuous-rotation servo (pulse width controls speed; ~1500us = stop)
SERVO_MODE = "CR"

# If PENUP/PENDOWN are reversed mechanically, set True.
SWAP_PEN = True

PEN_UP_ANGLE = 30
PEN_DOWN_ANGLE = 120

# Continuous rotation tuning (microseconds)
SERVO_STOP_US = 1500
PEN_UP_US = 1400
PEN_DOWN_US = 1600
PEN_UP_MS = 220
PEN_DOWN_MS = 220


def _servo_duty_us(us):
    # 50Hz period is 20,000us
    us = int(us)
    if us < 400:
        us = 400
    if us > 2600:
        us = 2600
    return int(us * 65535 / 20000)


def _servo_duty_angle(angle):
    # Typical range: 0.5ms..2.5ms over 0..180deg
    a = int(angle)
    if a < 0:
        a = 0
    if a > 180:
        a = 180
    us = 500 + int((a / 180) * 2000)
    return _servo_duty_us(us)


_servo_pwm = None
_servo_release_at = 0


def _servo_start_angle(angle, hold_ms=500):
    global _servo_pwm, _servo_release_at
    if _servo_pwm is None:
        _servo_pwm = PWM(Pin(SERVO_PIN))
        _servo_pwm.freq(50)
    _servo_pwm.duty_u16(_servo_duty_angle(angle))
    _servo_release_at = ticks_ms() + hold_ms


def _servo_start_us(us, hold_ms=300):
    global _servo_pwm, _servo_release_at
    if _servo_pwm is None:
        _servo_pwm = PWM(Pin(SERVO_PIN))
        _servo_pwm.freq(50)
    _servo_pwm.duty_u16(_servo_duty_us(us))
    _servo_release_at = ticks_ms() + hold_ms


def _servo_update():
    global _servo_pwm, _servo_release_at
    if _servo_pwm is None:
        return
    if ticks_diff(ticks_ms(), _servo_release_at) >= 0:
        # For continuous rotation, explicitly send stop pulse briefly.
        if SERVO_MODE == "CR":
            _servo_pwm.duty_u16(_servo_duty_us(SERVO_STOP_US))
            sleep_us(30_000)
        else:
            _servo_pwm.duty_u16(0)
        _servo_pwm.deinit()
        _servo_pwm = None


def pen_up():
    if SERVO_MODE == "CR":
        us = PEN_DOWN_US if SWAP_PEN else PEN_UP_US
        ms = PEN_DOWN_MS if SWAP_PEN else PEN_UP_MS
        _servo_start_us(us, ms)
    else:
        angle = PEN_DOWN_ANGLE if SWAP_PEN else PEN_UP_ANGLE
        _servo_start_angle(angle)
    print("OK PEN_UP")


def pen_down():
    if SERVO_MODE == "CR":
        us = PEN_UP_US if SWAP_PEN else PEN_DOWN_US
        ms = PEN_UP_MS if SWAP_PEN else PEN_DOWN_MS
        _servo_start_us(us, ms)
    else:
        angle = PEN_UP_ANGLE if SWAP_PEN else PEN_DOWN_ANGLE
        _servo_start_angle(angle)
    print("OK PEN_DOWN")


# ─── STEPPER ──────────────────────────────────────────────────────────────────
class Axis:
    def __init__(self, step_pin, dir_pin, name):
        self.step = step_pin
        self.dir = dir_pin
        self.name = name
        self._state = 0
        self._last = ticks_us()
        self._interval = 0
        self._remaining = 0

    def move(self, steps, interval_us):
        if steps == 0:
            return
        self.dir.value(1 if steps > 0 else 0)
        self._interval = max(interval_us, 200)
        self._remaining = abs(steps) * 2
        self._last = ticks_us()

    def stop(self):
        self._remaining = 0
        self._interval = 0
        self.step.value(0)
        self._state = 0

    @property
    def busy(self):
        return self._remaining > 0

    def update(self):
        if self._interval == 0 or self._remaining == 0:
            return
        now = ticks_us()
        if ticks_diff(now, self._last) >= self._interval:
            self._last = now
            self._state = 1 - self._state
            self.step.value(self._state)
            self._remaining -= 1
            if self._remaining == 0:
                self.step.value(0)
                self._state = 0


axis_x = Axis(STEP1, DIR1, "X")
axis_y = Axis(STEP2, DIR2, "Y")

# ─── HIZ AYARI ────────────────────────────────────────────────────────────────
DEFAULT_INTERVAL = 1200

# ─── KINEMATIK (COREXY) ───────────────────────────────────────────────────────
# Eğer "X dediğimde XY (diagonal) gidiyor" görüyorsan mekanik büyük ihtimal CoreXY/H-bot.
# CoreXY için fiziksel (X,Y) hareketini motor (A,B) adımlarına çeviriyoruz:
#   A = X + Y
#   B = X - Y
COREXY = True

# Eğer "X komutu fiziksel Y yapıyor" ve "Y komutu fiziksel X yapıyor" ise bunu True yap.
SWAP_XY = True

# Motor yönleri ters ise bu çarpanları -1 yap.
MOTOR_A_SIGN = 1
MOTOR_B_SIGN = 1


# ─── XY SENKRON STEP (BRESENHAM) ──────────────────────────────────────────────
def _pulse(step_pin, high_us, low_us):
    step_pin.value(1)
    sleep_us(high_us)
    step_pin.value(0)
    sleep_us(low_us)


def _xy_to_motors(x_steps, y_steps):
    xs = int(x_steps or 0)
    ys = int(y_steps or 0)
    if SWAP_XY:
        xs, ys = ys, xs
    if COREXY:
        a = (xs + ys) * MOTOR_A_SIGN
        b = (xs - ys) * MOTOR_B_SIGN
        return a, b
    return xs * MOTOR_A_SIGN, ys * MOTOR_B_SIGN


def move_xy_sync(x_steps, y_steps, interval=None):
    """
    Step-space line: interleaves motor steps with Bresenham-style error.
    If COREXY=True, input is physical X/Y, converted to motor A/B.
    interval: microseconds between STEP edges (approx). Lower=faster.
    """
    iv = interval or DEFAULT_INTERVAL
    iv = max(200, int(iv))
    high_us = iv // 2
    low_us = iv - high_us

    a_steps, b_steps = _xy_to_motors(x_steps, y_steps)

    if a_steps == 0 and b_steps == 0:
        return

    # Motor A -> (DIR1, STEP1), Motor B -> (DIR2, STEP2)
    DIR1.value(1 if a_steps > 0 else 0)
    DIR2.value(1 if b_steps > 0 else 0)

    aa = abs(a_steps)
    bb = abs(b_steps)

    # Major axis drives loop count.
    if aa >= bb:
        err = aa // 2
        for _ in range(aa):
            if axis_x._remaining == 0 and axis_y._remaining == 0:
                # not used; keep compat if someone calls stop_all()
                pass
            _pulse(STEP1, high_us, low_us)
            err -= bb
            if err < 0 and bb:
                _pulse(STEP2, high_us, low_us)
                err += aa
    else:
        err = bb // 2
        for _ in range(bb):
            _pulse(STEP2, high_us, low_us)
            err -= aa
            if err < 0 and aa:
                _pulse(STEP1, high_us, low_us)
                err += bb


# ─── HAREKET FONKSİYONLARI ───────────────────────────────────────────────────
def move_x(steps, interval=None):
    # Blocking move for predictable motion (used by Thonny + Web Serial)
    move_xy_sync(int(steps), 0, interval or DEFAULT_INTERVAL)


def move_y(steps, interval=None):
    move_xy_sync(0, int(steps), interval or DEFAULT_INTERVAL)


def move_xy(x_steps, y_steps, interval=None):
    move_xy_sync(x_steps, y_steps, interval or DEFAULT_INTERVAL)


def stop_all():
    global _servo_pwm, _servo_release_at
    axis_x.stop()
    axis_y.stop()
    if _servo_pwm is not None:
        _servo_pwm.duty_u16(0)
        _servo_pwm.deinit()
        _servo_pwm = None
    _servo_release_at = 0
    print("OK STOP")


def wait_idle():
    # With blocking moves, we are already idle after commands finish.
    _servo_update()


# ─── KOMUT PARSER ─────────────────────────────────────────────────────────────
def handle_command(raw):
    global DEFAULT_INTERVAL, PEN_UP_ANGLE, PEN_DOWN_ANGLE
    global SERVO_MODE, SERVO_STOP_US, PEN_UP_US, PEN_DOWN_US, PEN_UP_MS, PEN_DOWN_MS
    cmd = raw.strip().upper()
    if not cmd or cmd.startswith("#"):
        return

    if cmd == "PENUP":
        pen_up()
        return
    if cmd == "PENDOWN":
        pen_down()
        return
    if cmd == "STOP":
        stop_all()
        return
    if cmd == "WAIT":
        wait_idle()
        print("OK IDLE")
        return
    if cmd == "STATUS":
        print("OK X_BUSY={} Y_BUSY={}".format(False, False))
        return

    if cmd.startswith("SERVO "):
        try:
            val = int(cmd[6:].strip())
            if SERVO_MODE == "CR":
                _servo_start_us(val, 400)
                print("OK SERVO_US {}".format(val))
            else:
                _servo_start_angle(val, 1000)
                print("OK SERVO {}".format(val))
        except:
            print("ERR bad SERVO angle")
        return

    if cmd.startswith("SERVO_MODE "):
        mode = cmd[11:].strip().upper()
        if mode in ("ANGLE", "CR"):
            SERVO_MODE = mode
            print("OK SERVO_MODE={}".format(SERVO_MODE))
        else:
            print("ERR SERVO_MODE ANGLE|CR")
        return

    if cmd.startswith("SERVO_STOP_US "):
        try:
            SERVO_STOP_US = int(cmd[14:].strip())
            print("OK SERVO_STOP_US={}".format(SERVO_STOP_US))
        except:
            print("ERR bad SERVO_STOP_US")
        return

    if cmd.startswith("PENUP_US "):
        try:
            PEN_UP_US = int(cmd[9:].strip())
            print("OK PENUP_US={}".format(PEN_UP_US))
        except:
            print("ERR bad PENUP_US")
        return

    if cmd.startswith("PENDOWN_US "):
        try:
            PEN_DOWN_US = int(cmd[11:].strip())
            print("OK PENDOWN_US={}".format(PEN_DOWN_US))
        except:
            print("ERR bad PENDOWN_US")
        return

    if cmd.startswith("PENUP_MS "):
        try:
            PEN_UP_MS = int(cmd[9:].strip())
            print("OK PENUP_MS={}".format(PEN_UP_MS))
        except:
            print("ERR bad PENUP_MS")
        return

    if cmd.startswith("PENDOWN_MS "):
        try:
            PEN_DOWN_MS = int(cmd[11:].strip())
            print("OK PENDOWN_MS={}".format(PEN_DOWN_MS))
        except:
            print("ERR bad PENDOWN_MS")
        return

    if cmd.startswith("PENUP_ANGLE "):
        try:
            PEN_UP_ANGLE = int(cmd[12:].strip())
            print("OK PENUP_ANGLE={}".format(PEN_UP_ANGLE))
        except:
            print("ERR bad angle")
        return

    if cmd.startswith("PENDOWN_ANGLE "):
        try:
            PEN_DOWN_ANGLE = int(cmd[14:].strip())
            print("OK PENDOWN_ANGLE={}".format(PEN_DOWN_ANGLE))
        except:
            print("ERR bad angle")
        return

    if cmd.startswith("SPEED "):
        try:
            DEFAULT_INTERVAL = max(200, int(cmd[6:].strip()))
            print("OK SPEED={}".format(DEFAULT_INTERVAL))
        except:
            print("ERR bad SPEED")
        return

    if cmd.startswith("X") and not cmd.startswith("MOVE"):
        try:
            move_x(int(cmd[1:]))
            print("OK X {}".format(cmd[1:]))
        except:
            print("ERR bad X value")
        return

    if cmd.startswith("Y") and not cmd.startswith("MOVE"):
        try:
            move_y(int(cmd[1:]))
            print("OK Y {}".format(cmd[1:]))
        except:
            print("ERR bad Y value")
        return

    if cmd.startswith("MOVE "):
        rest = cmd[5:].strip().split()
        xv = yv = None
        for part in rest:
            if part.startswith("X"):
                try:
                    xv = int(part[1:])
                except:
                    print("ERR bad X in MOVE")
                    return
            elif part.startswith("Y"):
                try:
                    yv = int(part[1:])
                except:
                    print("ERR bad Y in MOVE")
                    return
        if xv is None and yv is None:
            print("ERR MOVE needs X or Y")
            return
        move_xy(xv or 0, yv or 0, DEFAULT_INTERVAL)
        print("OK MOVE X={} Y={}".format(xv, yv))
        return

    print("ERR unknown: {}".format(raw.strip()))


# ─── ANA LOOP ─────────────────────────────────────────────────────────────────
def main():
    import uselect

    print("PLOTTER READY")
    buf = ""
    try:
        p = uselect.poll()
        p.register(sys.stdin, uselect.POLLIN)
        use_poll = True
    except:
        use_poll = False

    while True:
        _servo_update()

        if use_poll:
            if p.poll(0):
                ch = sys.stdin.read(1)
                if ch in ("\n", "\r"):
                    if buf:
                        handle_command(buf)
                        buf = ""
                elif ch:
                    buf += ch
        else:
            try:
                if sys.stdin.any():
                    ch = sys.stdin.read(1)
                    if ch in ("\n", "\r"):
                        if buf:
                            handle_command(buf)
                            buf = ""
                    elif ch:
                        buf += ch
            except:
                pass


if __name__ == "__main__":
    main()