//! C parity tests for NTM (New Target Monitor) process path connection.

use motor_rs::flags::*;
use motor_rs::record::MotorRecord;

use asyn_rs::interfaces::motor::MotorStatus;
use epics_base_rs::server::record::Record;
use epics_base_rs::types::EpicsValue;

fn make_record() -> MotorRecord {
    let mut rec = MotorRecord::new();
    rec.conv.mres = 0.001;
    rec.limits.dhlm = 100.0;
    rec.limits.dllm = -100.0;
    rec.limits.hlm = 100.0;
    rec.limits.llm = -100.0;
    rec.limits.lvio = false;
    rec.vel.velo = 10.0;
    rec.vel.accl = 0.5;
    rec.vel.bvel = 5.0;
    rec.vel.bacc = 0.5;
    rec.stat.msta = MstaFlags::DONE;
    rec.timing.ntm = true;
    rec.timing.ntmf = 2.0;
    rec
}

fn complete_move(rec: &mut MotorRecord, target_pos: f64) {
    let status = MotorStatus {
        position: target_pos,
        encoder_position: target_pos,
        done: true,
        moving: false,
        ..Default::default()
    };
    rec.process_motor_info(&status);
}

fn motor_moving(rec: &mut MotorRecord, current_pos: f64) {
    let status = MotorStatus {
        position: current_pos,
        encoder_position: current_pos,
        done: false,
        moving: true,
        ..Default::default()
    };
    rec.process_motor_info(&status);
}

#[test]
fn val_during_motion_same_direction_accepted() {
    let mut rec = make_record();

    // Start move to 50
    rec.put_field("VAL", EpicsValue::Double(50.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 50.0;
    assert!(!rec.stat.dmov);
    assert_eq!(rec.stat.phase, MotionPhase::MainMove);

    // Motor moving at 25
    motor_moving(&mut rec, 25.0);

    // New target: 80 (same direction, farther) → ExtendMove.
    // Must re-emit a move command so the driver actually retargets.
    // Without this, the driver keeps the old target and motor stops
    // at the original target (RTRY=0 won't save us).
    rec.put_field("VAL", EpicsValue::Double(80.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // A new MoveAbsolute (or MoveRelative with equivalent target)
    // must be emitted to the driver at the new target.
    assert_eq!(effects.commands.len(), 1, "expected one move command");
    match &effects.commands[0] {
        MotorCommand::MoveAbsolute { position, .. } => {
            assert!(
                (*position - 80.0).abs() < 1e-6,
                "abs target should be 80, got {position}"
            );
        }
        MotorCommand::MoveRelative { distance, .. } => {
            // Relative move: current drbv is 25, so distance should be 55.
            assert!(
                (*distance - 55.0).abs() < 1e-6,
                "rel distance should be 55, got {distance}"
            );
        }
        other => panic!("expected Move command, got {other:?}"),
    }
    // C parity: ldvl is updated to the newly dispatched target
    // (motorRecord.cc:2469 load_pos on re-emitted move). This keeps
    // is_preferred_direction comparing against the most recent target
    // across successive in-flight retargets.
    assert_eq!(rec.internal.ldvl, 80.0);
    // FLNK must still be suppressed since motion continues.
    assert!(effects.suppress_forward_link);
    assert!(!rec.stat.dmov);
}

#[test]
fn opposite_direction_retarget_stops_and_replans() {
    let mut rec = make_record();

    // Start move to 50
    rec.put_field("VAL", EpicsValue::Double(50.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 50.0;

    motor_moving(&mut rec, 25.0);

    // New target: -20 (opposite direction) → StopAndReplan
    rec.put_field("VAL", EpicsValue::Double(-20.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // Should issue Stop and store pending retarget
    assert_eq!(effects.commands.len(), 1);
    assert!(matches!(effects.commands[0], MotorCommand::Stop { .. }));
    assert!(rec.stat.mip.contains(MipFlags::STOP));
    assert_eq!(rec.internal.pending_retarget, Some(-20.0));

    // Motor stops
    complete_move(&mut rec, 25.0);
    let effects = rec.check_completion();

    // Should replan to -20
    assert!(!rec.stat.dmov);
    assert_eq!(rec.stat.phase, MotionPhase::MainMove);
    assert_eq!(effects.commands.len(), 1);
    if let MotorCommand::MoveAbsolute { position, .. } = &effects.commands[0] {
        assert!((*position - (-20.0)).abs() < 1e-10);
    } else {
        panic!("expected MoveAbsolute for replan");
    }
}

#[test]
fn ntm_false_ignores_retarget() {
    let mut rec = make_record();
    rec.timing.ntm = false;

    rec.put_field("VAL", EpicsValue::Double(50.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 50.0;

    motor_moving(&mut rec, 25.0);

    // New target while NTM=false → ignored
    rec.put_field("VAL", EpicsValue::Double(80.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // Should be ignored (no commands)
    assert!(effects.commands.is_empty());
}

#[test]
fn retarget_during_backlash_cancels_pending() {
    let mut rec = make_record();
    rec.retry.bdst = 1.0;

    // Start move with backlash (moving negative with positive BDST)
    rec.put_field("VAL", EpicsValue::Double(-10.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = -10.0;
    assert!(rec.internal.backlash_pending);
    assert_eq!(rec.stat.phase, MotionPhase::MainMove);

    motor_moving(&mut rec, -5.0);

    // Explicitly set CDIR to match the negative move direction
    // (CDIR may be overwritten by process_motor_info's TDIR update)
    rec.stat.cdir = false; // moving negative

    // Retarget to opposite direction (20.0 vs original -10.0)
    rec.put_field("VAL", EpicsValue::Double(20.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // Opposite direction retarget: stop-and-replan
    assert!(rec.stat.mip.contains(MipFlags::STOP));
    assert!(!rec.internal.backlash_pending);
    assert!(matches!(effects.commands[0], MotorCommand::Stop { .. }));
}

#[test]
fn retarget_during_retry_resets_rcnt() {
    let mut rec = make_record();
    rec.retry.rdbd = 0.1;
    rec.retry.rtry = 5;
    rec.retry.rmod = RetryMode::Geometric;

    rec.put_field("VAL", EpicsValue::Double(10.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 10.0;

    // Enter retry
    complete_move(&mut rec, 9.5);
    rec.check_completion();
    assert_eq!(rec.stat.phase, MotionPhase::Retry);
    assert_eq!(rec.retry.rcnt, 1);

    // Motor retrying
    motor_moving(&mut rec, 9.7);

    // Retarget to opposite direction during retry (requires stop-and-replan)
    rec.put_field("VAL", EpicsValue::Double(-20.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // RCNT should be reset, stop issued
    assert_eq!(rec.retry.rcnt, 0);
    assert!(matches!(effects.commands[0], MotorCommand::Stop { .. }));
}

#[test]
fn stop_and_replan_with_backlash() {
    let mut rec = make_record();
    rec.retry.bdst = 1.0;

    // Start move to 50
    rec.put_field("VAL", EpicsValue::Double(50.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 50.0;

    motor_moving(&mut rec, 25.0);

    // Retarget to -10 (opposite direction, needs backlash)
    rec.put_field("VAL", EpicsValue::Double(-10.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // Should stop
    assert!(matches!(effects.commands[0], MotorCommand::Stop { .. }));
    assert_eq!(rec.internal.pending_retarget, Some(-10.0));

    // Motor stops, replan happens
    complete_move(&mut rec, 25.0);
    let effects = rec.check_completion();

    // Replan should include backlash (moving negative with BDST=+1)
    assert_eq!(rec.stat.phase, MotionPhase::MainMove);
    assert!(rec.internal.backlash_pending);
    if let MotorCommand::MoveAbsolute { position, .. } = &effects.commands[0] {
        // Pretarget = -10 - 1 = -11
        assert!((*position - (-11.0)).abs() < 1e-10);
    } else {
        panic!("expected MoveAbsolute");
    }
}

#[test]
fn spmg_pause_stops_and_syncs_after_completion() {
    let mut rec = make_record();

    // Start move
    rec.put_field("VAL", EpicsValue::Double(50.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    motor_moving(&mut rec, 25.0);

    // Pause: sends STOP, sets MIP_STOP, but motor is still moving
    rec.ctrl.spmg = SpmgMode::Pause;
    let effects = rec.plan_motion(CommandSource::Spmg);
    assert!(!rec.stat.dmov); // motor still moving
    assert!(rec.stat.mip.contains(MipFlags::STOP));
    assert!(matches!(effects.commands[0], MotorCommand::Stop { .. }));

    // Motor stops at position 25.0
    complete_move(&mut rec, 25.0);
    let _effects = rec.check_completion();

    // C: postProcess syncs positions (VAL=RBV, DVAL=DRBV)
    assert!(rec.stat.dmov);
    assert_eq!(rec.pos.dval, 25.0); // synced to readback
    assert_eq!(rec.pos.val, 25.0);

    // Go: no resume since positions are synced
    rec.ctrl.spmg = SpmgMode::Go;
    rec.internal.lspg = SpmgMode::Pause;
    let effects = rec.plan_motion(CommandSource::Spmg);
    assert!(effects.commands.is_empty()); // no move, already at target
}

#[test]
fn same_direction_retarget_reaches_new_target_without_retry() {
    // Regression: caput 25 then (mid-motion) caput 45 on same direction must
    // cause the motor to actually reach 45, not stop at 25. Previously the
    // ExtendMove branch updated DVAL silently and the driver kept going to
    // 25, so with RTRY=0 (no retry to save us) the motor stopped at 25.
    let mut rec = make_record();
    rec.retry.rtry = 0; // disable retry to prove we don't depend on it

    // Move from 0 to 25
    rec.put_field("VAL", EpicsValue::Double(25.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);
    assert_eq!(effects.commands.len(), 1);
    rec.internal.ldvl = 25.0;

    // Motor moving at 15 (mid-flight)
    motor_moving(&mut rec, 15.0);
    assert!(!rec.stat.dmov);

    // User retargets to 45 (same direction, further)
    rec.put_field("VAL", EpicsValue::Double(45.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);

    // Must issue a new move command to the new target.
    assert_eq!(
        effects.commands.len(),
        1,
        "retarget must emit a new move command"
    );
    let new_target = match &effects.commands[0] {
        MotorCommand::MoveAbsolute { position, .. } => *position,
        MotorCommand::MoveRelative { distance, .. } => 15.0 + distance,
        other => panic!("expected Move command, got {other:?}"),
    };
    assert!(
        (new_target - 45.0).abs() < 1e-6,
        "new target should be 45, got {new_target}"
    );

    // Simulate driver accepting the retarget and reaching 45.
    complete_move(&mut rec, 45.0);
    let _effects = rec.check_completion();

    assert!(rec.stat.dmov, "motor should be done after reaching 45");
    assert!(
        (rec.pos.drbv - 45.0).abs() < 1e-6,
        "DRBV should be 45, got {}",
        rec.pos.drbv
    );
}

#[test]
fn same_direction_retarget_safety_net_replans_if_driver_ignores_inflight() {
    // Safety net: even if the driver silently ignores the in-flight retarget
    // and stops at the OLD target, completion-time verification must replan
    // to the new target (without depending on RTRY/RDBD).
    let mut rec = make_record();
    rec.retry.rtry = 0; // no retries available
    rec.retry.rdbd = 0.0; // retry gating disabled

    // Start move to 25
    rec.put_field("VAL", EpicsValue::Double(25.0)).unwrap();
    rec.plan_motion(CommandSource::Val);
    rec.internal.ldvl = 25.0;

    motor_moving(&mut rec, 15.0);

    // Mid-motion retarget to 45 (same direction) → ExtendMove arms safety net
    rec.put_field("VAL", EpicsValue::Double(45.0)).unwrap();
    let effects = rec.plan_motion(CommandSource::Val);
    assert_eq!(effects.commands.len(), 1, "ExtendMove must emit move");
    assert!(
        rec.internal.verify_retarget_on_completion,
        "verify flag must be armed"
    );

    // Simulate driver IGNORING the in-flight retarget and stopping at 25.
    complete_move(&mut rec, 25.0);
    let effects = rec.check_completion();

    // Safety net must have replanned to 45.
    assert!(
        !rec.internal.verify_retarget_on_completion,
        "verify flag must be cleared after check"
    );
    assert!(
        !rec.stat.dmov,
        "motor must NOT be marked done — replan issued"
    );
    assert_eq!(
        effects.commands.len(),
        1,
        "safety-net replan must emit a move command"
    );
    let new_target = match &effects.commands[0] {
        MotorCommand::MoveAbsolute { position, .. } => *position,
        MotorCommand::MoveRelative { distance, .. } => 25.0 + distance,
        other => panic!("expected Move command, got {other:?}"),
    };
    assert!(
        (new_target - 45.0).abs() < 1e-6,
        "safety-net target should be 45, got {new_target}"
    );

    // Driver reaches 45 on second attempt.
    complete_move(&mut rec, 45.0);
    let _effects = rec.check_completion();
    assert!(rec.stat.dmov);
    assert!((rec.pos.drbv - 45.0).abs() < 1e-6);
}
