# lijnvolger.py - een robot met twee wielen volgt de lus in de gang (P-regelaar of aan/uit)
import math

B = 0.14                  # m tussen de wielen
A = 0.06                  # m: de reflectiesensoren zitten voor de as
V_MAX, D0 = 0.60, 0.18    # m/s bij PWM 255; dode zone van de motoren
TAU = 0.08                # s: zo snel volgt een wiel een nieuwe snelheid
DT, T_REGEL = 0.002, 0.02
XA, XB, YA, YB, YC, R = 0.45, 1.40, 0.55, 0.25, 0.40, 0.15    # de lus (m)


def tot_lijn(x, y):
    d = 9.0
    if XA <= x <= XB:
        d = min(abs(y - YA), abs(y - YB))
    if x >= XB:
        d = min(d, abs(math.hypot(x - XB, y - YC) - R))
    if x <= XA:
        d = min(d, abs(math.hypot(x - XA, y - YC) - R))
    return d


def zwart(dy, x, y, th):
    """Hoe zwart ziet de sensor op dy (m, + = links) het? 0 = wit, 1 = zwart (tape 19 mm, vlekje 8 mm)."""
    sx = x + A * math.cos(th) - dy * math.sin(th)
    sy = y + A * math.sin(th) + dy * math.cos(th)
    return min(1.0, max(0.0, (0.0135 - tot_lijn(sx, sy)) / 0.008))


def wiel(v):
    """Wat het wiel echt doet: de code overbrugt de dode zone, PWM in hele stappen."""
    if v == 0:
        return 0.0
    pwm = round(255 * (D0 + (1 - D0) * min(abs(v) / V_MAX, 1.0)))
    return math.copysign(V_MAX * max(0.0, pwm / 255 - D0) / (1 - D0), v)


def rij(regelaar, K=0.25, v0=0.20, x=0.55, y=0.56, th=0.0, T=10.0):
    vl = vr = doel_l = doel_r = 0.0
    kant, pad = 1, []
    for k in range(int(T / DT)):
        if k % int(T_REGEL / DT) == 0:                       # 50 keer per seconde beslissen
            w = [zwart(dy, x, y, th) for dy in (0.015, 0.0, -0.015)]
            if regelaar == "P":
                if sum(w) > 0.2:
                    pos = (w[0] * 15 - w[2] * 15) / sum(w)   # mm, + = lijn links
                    kant = 1 if pos > 0 else -1
                else:
                    pos = 15 * kant                          # kwijt: zoek aan de laatste kant
                om = K * pos
                doel_l, doel_r = v0 - om * B / 2, v0 + om * B / 2
            else:                                            # aan/uit met drempel 0,5
                links, midden, rechts = (s > 0.5 for s in w)
                if links and not rechts:
                    kant = 1
                elif rechts and not links:
                    kant = -1
                if midden and not (links or rechts):
                    doel_l = doel_r = v0
                else:                                        # bijsturen naar de kant van de lijn
                    doel_l, doel_r = (0.5 * v0, 1.2 * v0) if kant > 0 else (1.2 * v0, 0.5 * v0)
        vl += (wiel(doel_l) - vl) * DT / TAU
        vr += (wiel(doel_r) - vr) * DT / TAU
        v, om = (vl + vr) / 2, (vr - vl) / B
        x += v * math.cos(th) * DT
        y += v * math.sin(th) * DT
        th += om * DT
        pad.append((k * DT, x, y, tot_lijn(x, y)))
    return pad


if __name__ == "__main__":
    for naam, K in (("aan/uit", 0), ("P, K = 0,08", 0.08), ("P, K = 0,25", 0.25), ("P, K = 1,5", 1.5)):
        pad = rij("aanuit" if K == 0 else "P", K)
        afw = [p[3] * 1000 for p in pad if p[0] > 0.5]
        weg = sum(math.hypot(b[1] - a[1], b[2] - a[2]) for a, b in zip(pad, pad[1:]))
        print(f"{naam:12} grootste afwijking {max(afw):5.1f} mm, gemiddeld {sum(afw) / len(afw):4.1f} mm, afgelegd {weg:.2f} m")