const PROXIMITY_EPSILON = 1e-6

export function needsPrismRefresh(agent, now, prismTimeHeight, prismRefreshLeadSeconds = 0) {
    return(now >= agent.since + prismTimeHeight - prismRefreshLeadSeconds)
}

export function advanceAgentSegment(agent, now) {
    agent.position = positionAt(agent, now)
    agent.since = now
    agent.generation = (agent.generation ?? 0) + 1
}

export function positionAt(agent, t) {
    const dt = t - agent.since
    return({
        x: agent.position.x + agent.vector.x * dt,
        y: agent.position.y + agent.vector.y * dt,
        z: agent.position.z + agent.vector.z * dt,
    })
}

export function distanceBetween(agentA, agentB, t) {
    const a = positionAt(agentA, t)
    const b = positionAt(agentB, t)
    const dx = a.x - b.x
    const dy = a.y - b.y
    const dz = a.z - b.z
    return(Math.sqrt(dx * dx + dy * dy + dz * dz))
}

export function buildPrism(agent, tStart, tEnd, nearMissDistance) {
    const p0 = positionAt(agent, tStart)
    const p1 = positionAt(agent, tEnd)
    const pad = nearMissDistance
    return({
        xMin: Math.min(p0.x, p1.x) - pad,
        xMax: Math.max(p0.x, p1.x) + pad,
        yMin: Math.min(p0.y, p1.y) - pad,
        yMax: Math.max(p0.y, p1.y) + pad,
        zMin: Math.min(p0.z, p1.z) - pad,
        zMax: Math.max(p0.z, p1.z) + pad,
        tMin: tStart,
        tMax: tEnd,
    })
}

export function prismsIntersect(a, b) {
    return(
        a.xMin <= b.xMax && a.xMax >= b.xMin &&
        a.yMin <= b.yMax && a.yMax >= b.yMin &&
        a.zMin <= b.zMax && a.zMax >= b.zMin &&
        a.tMin <= b.tMax && a.tMax >= b.tMin
    )
}

export function minimalDistance(agentA, agentB, tStart, tEnd) {
    const relPos = {
        x: positionAt(agentA, tStart).x - positionAt(agentB, tStart).x,
        y: positionAt(agentA, tStart).y - positionAt(agentB, tStart).y,
        z: positionAt(agentA, tStart).z - positionAt(agentB, tStart).z,
    }
    const relVel = {
        x: agentA.vector.x - agentB.vector.x,
        y: agentA.vector.y - agentB.vector.y,
        z: agentA.vector.z - agentB.vector.z,
    }

    const relVelSq = relVel.x * relVel.x + relVel.y * relVel.y + relVel.z * relVel.z
    let tStar = tStart
    if(relVelSq > 0) {
        const dot = relPos.x * relVel.x + relPos.y * relVel.y + relPos.z * relVel.z
        tStar = tStart - dot / relVelSq
    }
    tStar = Math.max(tStart, Math.min(tEnd, tStar))

    return({
        distance: distanceBetween(agentA, agentB, tStar),
        time: tStar,
    })
}

export function firstProximityEntry(agentA, agentB, tStart, tEnd, nearMissDistance) {
    const d0 = distanceBetween(agentA, agentB, tStart)
    if(d0 <= nearMissDistance + PROXIMITY_EPSILON) {
        return({ time: tStart, distance: d0 })
    }

    const relPos = {
        x: positionAt(agentA, tStart).x - positionAt(agentB, tStart).x,
        y: positionAt(agentA, tStart).y - positionAt(agentB, tStart).y,
        z: positionAt(agentA, tStart).z - positionAt(agentB, tStart).z,
    }
    const relVel = {
        x: agentA.vector.x - agentB.vector.x,
        y: agentA.vector.y - agentB.vector.y,
        z: agentA.vector.z - agentB.vector.z,
    }
    const relVelSq = relVel.x * relVel.x + relVel.y * relVel.y + relVel.z * relVel.z
    if(relVelSq === 0) return(null)

    const a = relVelSq
    const b = 2 * (relPos.x * relVel.x + relPos.y * relVel.y + relPos.z * relVel.z)
    const c = relPos.x * relPos.x + relPos.y * relPos.y + relPos.z * relPos.z - nearMissDistance * nearMissDistance
    const D = b * b - 4 * a * c
    if(D < 0) return(null)

    const sqrtD = Math.sqrt(D)
    const maxDt = tEnd - tStart
    const roots = [(-b - sqrtD) / (2 * a), (-b + sqrtD) / (2 * a)]
        .filter(dt => dt >= 0 && dt <= maxDt)
        .sort((x, y) => x - y)

    for(const dt of roots) {
        const t = tStart + dt
        const d = distanceBetween(agentA, agentB, t)
        if(d > nearMissDistance + PROXIMITY_EPSILON) continue
        if(dt < 1e-12) return({ time: tStart, distance: d0 })
        const dBefore = distanceBetween(agentA, agentB, t - 1e-6)
        if(dBefore > nearMissDistance + PROXIMITY_EPSILON) return({ time: t, distance: d })
    }
    return(null)
}

export function compareWorldlinePair(agentA, agentB, prismTimeHeight, nearMissDistance, now) {
    const tStart = Math.max(agentA.since, agentB.since, now)
    const tEnd = Math.min(
        agentA.since + prismTimeHeight,
        agentB.since + prismTimeHeight
    )
    if(tEnd <= tStart) return(null)

    const prismA = buildPrism(agentA, tStart, tEnd, nearMissDistance)
    const prismB = buildPrism(agentB, tStart, tEnd, nearMissDistance)
    if(!prismsIntersect(prismA, prismB)) return(null)

    const min = minimalDistance(agentA, agentB, tStart, tEnd)
    if(min.distance > nearMissDistance + PROXIMITY_EPSILON) return(null)

    const entry = firstProximityEntry(agentA, agentB, tStart, tEnd, nearMissDistance)
    if(!entry) return(null)

    return({
        agentA: agentA.id,
        agentB: agentB.id,
        time: entry.time,
        distance: entry.distance,
        minTime: min.time,
        minDistance: min.distance,
    })
}