// File: /workspace/github_cpp_makefile_star2_O2/sundale[P]gcode/rs274ngc/interp_queue.cc
// Function: Interp::move_endpoint_and_flush(setup_struct*, double, double)
int Interp::move_endpoint_and_flush(setup_pointer settings, double x, double y) {
    double x1;
    double y1;
    double x2;
    double y2;
    double dot;

    if(qc().empty()) return 0;
    
    for(unsigned int i = 0; i<qc().size(); i++) {
        // there may be several moves in the queue, and we need to
        // change all of them.  consider moving into a concave corner,
        // then up and back down, then continuing on.  there will be
        // three moves to change.

        queued_canon &q = qc()[i];

        switch(q.type) {
        case QARC_FEED:
            double r1, r2, l1, l2;
            r1 = hypot(q.data.arc_feed.end1 - q.data.arc_feed.center1,
                       q.data.arc_feed.end2 - q.data.arc_feed.center2);
            l1 = q.data.arc_feed.original_turns;
            q.data.arc_feed.end1 = x;
            q.data.arc_feed.end2 = y;
            r2 = hypot(x - q.data.arc_feed.center1,
                       y - q.data.arc_feed.center2);
            l2 = find_turn(endpoint[0], endpoint[1],
                           q.data.arc_feed.center1, q.data.arc_feed.center2,
                           q.data.arc_feed.turn,
                           x, y);
            if(debug_qc) printf("moving endpoint of arc lineno %d old sweep %f new speed %f\n", q.data.arc_feed.line_number, l1, l2);

            if(fabs(r1-r2) > .01) 
                ERS(_("BUG: cutter compensation has generated an invalid arc with mismatched radii r1 %f r2 %f\n"), r1, r2);
            if(l1 && endpoint_valid && fabs(l2) > fabs(l1) + (settings->length_units == CANON_UNITS_MM? .0254 : .001)) {
                ERS(_("Arc move in concave corner cannot be reached by the tool without gouging"));
            }
            q.data.arc_feed.end1 = x;
            q.data.arc_feed.end2 = y;
            break;
        case QSTRAIGHT_TRAVERSE:
            switch(settings->plane) {
            case CANON_PLANE_XY:
                x1 = q.data.straight_traverse.dx; // direction of original motion
                y1 = q.data.straight_traverse.dy;                
                x2 = x - endpoint[0];         // new direction after clipping
                y2 = y - endpoint[1];
                break;
            case CANON_PLANE_XZ:
                x1 = q.data.straight_traverse.dz; // direction of original motion
                y1 = q.data.straight_traverse.dx;                
                x2 = x - endpoint[0];         // new direction after clipping
                y2 = y - endpoint[1];
                break;
            default:
                ERS(_("BUG: Unsupported plane in cutter compensation"));
            }
            
            dot = x1 * x2 + y1 * y2; // not normalized; we only care about the angle
            if(debug_qc) printf("moving endpoint of traverse old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1,x1), atan2(y2,x2), dot, endpoint_valid);

            if(endpoint_valid && dot<0) {
                // oops, the move is the wrong way.  this means the
                // path has crossed because we backed up further
                // than the line is long.  this will gouge.
                ERS(_("Straight traverse in concave corner cannot be reached by the tool without gouging"));
            }
            switch(settings->plane) {
            case CANON_PLANE_XY:
                q.data.straight_traverse.x = x;
                q.data.straight_traverse.y = y;
                break;
            case CANON_PLANE_XZ:
                q.data.straight_traverse.z = x;
                q.data.straight_traverse.x = y;
                break;
            }
            break;
        case QSTRAIGHT_FEED: 
            switch(settings->plane) {
            case CANON_PLANE_XY:
                x1 = q.data.straight_feed.dx; // direction of original motion
                y1 = q.data.straight_feed.dy;                
                x2 = x - endpoint[0];         // new direction after clipping
                y2 = y - endpoint[1];
                break;
            case CANON_PLANE_XZ:
                x1 = q.data.straight_feed.dz; // direction of original motion
                y1 = q.data.straight_feed.dx;                
                x2 = x - endpoint[0];         // new direction after clipping
                y2 = y - endpoint[1];
                break;
            default:
                ERS(_("BUG: Unsupported plane [%d] in cutter compensation"),
			settings->plane);
            }

            dot = x1 * x2 + y1 * y2;
            if(debug_qc) printf("moving endpoint of feed old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1,x1), atan2(y2,x2), dot, endpoint_valid);

            if(endpoint_valid && dot<0) {
                // oops, the move is the wrong way.  this means the
                // path has crossed because we backed up further
                // than the line is long.  this will gouge.
                ERS(_("Straight feed in concave corner cannot be reached by the tool without gouging"));
            }
            switch(settings->plane) {
            case CANON_PLANE_XY:
                q.data.straight_feed.x = x;
                q.data.straight_feed.y = y;
                break;
            case CANON_PLANE_XZ:
                q.data.straight_feed.z = x;
                q.data.straight_feed.x = y;
                break;
            }
            break;
        default:
            // other things are not moves - we don't have to mess with them.
            ;
        }
    }
    dequeue_canons(settings);
    set_endpoint(x, y);
    return 0;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movsd %xmm1, -0x40(%rbp)
movsd %xmm0, -0x30(%rbp)
movq %rsi, %r14
movq %rdi, -0x38(%rbp)
callq 0xb240
movq 0x422cc(%rip), %rax     # 0x5c430
cmpq 0x422cd(%rip), %rax     # 0x5c438
je 0x1a3fb
xorl %r15d, %r15d
pushq $0x78
popq %r12
pushq $0x10
popq %r13
movl %r15d, %r15d
callq 0xb240
movq 0x422ad(%rip), %rax     # 0x5c438
subq 0x4229e(%rip), %rax     # 0x5c430
cqto
idivq %r12
cmpq %r15, %rax
jbe 0x1a3d2
callq 0xb240
movq 0x42284(%rip), %rax     # 0x5c430
imulq $0x78, %r15, %rcx
leaq (%rax,%rcx), %rbx
movl (%rax,%rcx), %eax
testl %eax, %eax
je 0x1a339
cmpl $0x1, %eax
je 0x1a2d4
cmpl $0x2, %eax
jne 0x1a3ca
movsd 0x18(%rbx), %xmm0
movsd 0x20(%rbx), %xmm1
subsd 0x28(%rbx), %xmm0
subsd 0x30(%rbx), %xmm1
callq 0xb2c0
movsd 0x10(%rbx), %xmm1
movaps %xmm1, -0x60(%rbp)
movsd -0x30(%rbp), %xmm2
movsd %xmm2, 0x18(%rbx)
movsd -0x40(%rbp), %xmm1
movsd %xmm1, 0x20(%rbx)
subsd 0x28(%rbx), %xmm2
movsd %xmm0, -0x50(%rbp)
subsd 0x30(%rbx), %xmm1
movapd %xmm2, %xmm0
callq 0xb2c0
movsd %xmm0, -0x48(%rbp)
movsd 0x42234(%rip), %xmm0    # 0x5c460
movsd 0x4223c(%rip), %xmm1    # 0x5c470
movsd 0x28(%rbx), %xmm2
movsd 0x30(%rbx), %xmm3
movl 0x38(%rbx), %esi
movq -0x38(%rbp), %rdi
movsd -0x30(%rbp), %xmm4
movsd -0x40(%rbp), %xmm5
callq 0xbc80
movsd -0x50(%rbp), %xmm1
subsd -0x48(%rbp), %xmm1
andpd 0x13dba(%rip), %xmm1    # 0x2e020
ucomisd 0x185d2(%rip), %xmm1    # 0x32840
ja 0x1a402
movapd -0x60(%rbp), %xmm2
ucomisd 0x15c4f(%rip), %xmm2    # 0x2fed0
jne 0x1a285
jnp 0x1a2c1
testb $0x1, 0x421c4(%rip)     # 0x5c450
je 0x1a2c1
movapd 0x13d8a(%rip), %xmm1    # 0x2e020
andpd %xmm1, %xmm0
andpd %xmm1, %xmm2
xorl %eax, %eax
cmpl $0x2, 0x1690(%r14)
sete %al
leaq 0x1857e(%rip), %rcx     # 0x32830
addsd (%rcx,%rax,8), %xmm2
ucomisd %xmm2, %xmm0
ja 0x1a466
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x18(%rbx)
addq $0x20, %rbx
jmp 0x1a3c1
movl 0xc67c(%r14), %eax
movq %r13, %rcx
pushq $0x18
popq %rdx
cmpl $0x1, %eax
je 0x1a2f5
cmpl $0x3, %eax
jne 0x1a43a
pushq $0x20
popq %rcx
movq %r13, %rdx
cmpb $0x1, 0x42154(%rip)     # 0x5c450
jne 0x1a399
movsd -0x30(%rbp), %xmm0
subsd 0x42151(%rip), %xmm0    # 0x5c460
movsd -0x40(%rbp), %xmm1
subsd 0x42154(%rip), %xmm1    # 0x5c470
mulsd (%rbx,%rdx), %xmm1
mulsd (%rbx,%rcx), %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x1a399
jmp 0x1a428
movl 0xc67c(%r14), %eax
movq %r13, %rcx
pushq $0x18
popq %rdx
cmpl $0x1, %eax
je 0x1a35a
cmpl $0x3, %eax
jne 0x1a45d
pushq $0x20
popq %rcx
movq %r13, %rdx
cmpb $0x1, 0x420ef(%rip)     # 0x5c450
jne 0x1a399
movsd -0x30(%rbp), %xmm0
subsd 0x420f0(%rip), %xmm0    # 0x5c460
movsd -0x40(%rbp), %xmm1
subsd 0x420f3(%rip), %xmm1    # 0x5c470
mulsd (%rbx,%rdx), %xmm1
mulsd (%rbx,%rcx), %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
ja 0x1a431
cmpl $0x3, %eax
je 0x1a3b3
cmpl $0x1, %eax
jne 0x1a3ca
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x28(%rbx)
addq $0x30, %rbx
jmp 0x1a3c1
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x38(%rbx)
addq $0x28, %rbx
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rbx)
incl %r15d
jmp 0x1a17c
movq %r14, %rdi
callq 0xb340
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x42079(%rip)    # 0x5c460
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x4207c(%rip)    # 0x5c470
movb $0x1, 0x42055(%rip)     # 0x5c450
xorl %eax, %eax
jmp 0x1a4b2
leaq 0x1843f(%rip), %rdi     # 0x32848
callq 0xb220
movq -0x38(%rbp), %rdi
movq %rax, %rsi
movsd -0x50(%rbp), %xmm0
movsd -0x48(%rbp), %xmm1
movb $0x2, %al
callq 0xb8e0
jmp 0x1a480
leaq 0x18525(%rip), %rdi     # 0x32954
jmp 0x1a46d
leaq 0x18497(%rip), %rdi     # 0x328cf
jmp 0x1a46d
leaq 0x184e0(%rip), %rdi     # 0x32921
callq 0xb220
movl 0xc67c(%r14), %edx
movq -0x38(%rbp), %rdi
movq %rax, %rsi
xorl %eax, %eax
callq 0xb8e0
jmp 0x1a480
leaq 0x1843d(%rip), %rdi     # 0x328a1
jmp 0x1a46d
leaq 0x16e26(%rip), %rdi     # 0x31293
callq 0xb220
movq -0x38(%rbp), %rdi
movq %rax, %rsi
xorl %eax, %eax
callq 0xb8e0
movq 0x22ad9(%rip), %rbx     # 0x3cf60
leaq 0xc6d0(%rbx), %rdi
leaq 0x18347(%rip), %rsi     # 0x327dc
pushq $0x50
popq %rdx
callq 0xb940
andw $0x0, 0xc71f(%rbx)
movl $0x1, 0xd670(%rbx)
pushq $0x5
popq %rax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq