// File: /workspace/github_c_makefile_star2_O3/ugobas[P]tnm/kinetic_tnm.c
// Function: Eckart_main
void Eckart_main(struct axe *axes, int N_axes, atom *atoms, int N_atoms,
		 struct Reference Ref)
{
  /******************************************************************/
  /*   Computing internal rotations and translations (Eckart frame)  */
  /******************************************************************/
  // Initialize
  int i, j, k;

 // Compute inertia tensor and center of mass for all atoms
  double **corr_sum=Allocate_mat2_d(3,3), mr_tot[3];
  for(i=0; i<3; i++)mr_tot[i]=0;
  double M_tot=Sum_inertia(mr_tot, corr_sum, atoms, Ref, 0, Ref.N_ref-1);
  double r_ave[3]; for(i=0; i<3; i++)r_ave[i]=mr_tot[i]/M_tot;
  if(START){
    printf("Center of mass: ");
    for(i=0; i<3; i++)printf("  %.7f", r_ave[i]);
    printf("   M=%.0f\n", M_tot);
    START=0;
  }

  double **inertia_tot=Allocate_mat2_d(3,3);
  Inertia_tensor(inertia_tot, corr_sum, mr_tot, M_tot);

  // Cholevsky decomposition of global inertia tensor
  double **L_mat=Allocate_mat2_d(3,3);
  choldc(L_mat, inertia_tot, 3);

  // Partial inertia tensor for all axes
  // Exploits the fact that axes are nested
  double M_part=0, mr_part[3], **inertia_part=Allocate_mat2_d(3,3);
  Empty_inertia(mr_part, corr_sum);

  struct axe *axe=axes+N_axes-1;
  int last_kin=-1, i_last=0;
  for(int k=N_axes-1; k>=0; k--){
    // Compute partial center of mass and inertia tensor
    if(axe->last_kin!=last_kin){
      // New chain, initialize counters
      M_part=Empty_inertia(mr_part, corr_sum);
      last_kin=axe->last_kin;
      i_last=last_kin;
    }
    M_part+=Sum_inertia(mr_part, corr_sum, atoms, Ref, axe->first_kin, i_last);
    Inertia_tensor(inertia_part, corr_sum, mr_part, M_part);
    i_last=axe->first_kin-1;

    // Test that at least some atom is moved
    if(M_part==0){
      printf("WARNING, axis %d does not move any reference atom!\n", k);
      printf("Reference atoms: %d - %d\n", axe->first_kin, axe->last_kin);
      printf("All atoms: %d - %d\n", axe->first_atom, axe->last_atom);
      printf("dof: %d-%d (main) ", axe->first_main, axe->last_main);
      if(axe->first_side>=0)printf(" %d-%d (side)", axe->first_side, k);
      printf("\n");
      //exit(8);
    }
 
    /*
      ECKART ROTATION AND SHIFT OPERATORS:
      Vector global rotation about axe k:
      Iner global_rot[k]=-iner_part[k]v[k]+M_k(R-Rk)X(axe[k]X Rk+tau[k])}
      Vector global shift:
      global_shift[k]= - (axe[k]X Rk+tau[k])Mk/M - global_rot[k] X R
     */

    // Center of mass displacement
    double delta_cm[3], r_shift[3], rot_tot[3], X[3], Y[3];
    for(i=0; i<3; i++)r_shift[i]=M_part*mr_tot[i]-mr_part[i];
    Vector_product_d(delta_cm, axe->rot, mr_part);
    for(i=0; i<3; i++)delta_cm[i]=delta_cm[i]/M_part+axe->shift[i];
    for(i=0; i<3; i++)r_shift[i]=M_part*r_ave[i]-mr_part[i];
    Vector_product_d(Y, r_shift, delta_cm);
    for(i=0; i<3; i++){
      for(j=0; j<3; j++)Y[i]-=inertia_part[i][j]*axe->rot[j];
    }

    // Downstream rotataion: Solve (L L^t)A_a = Y_a
    Forward_substitution(X, L_mat, Y, 3);
    Backward_substitution(axe->global_rot, L_mat, X, 3);

    // Downstream shift
    Vector_product_d(rot_tot, axe->global_rot, r_ave);
    for(i=0; i<3; i++){
      axe->global_shift[i]=-delta_cm[i]*M_part/M_tot-rot_tot[i]; //
    }

    // Downstream rotation and shift
    for(i=0; i<3; i++){
      axe->local_rot[i]  =axe->global_rot[i] + axe->rot[i];
      axe->local_shift[i]=axe->global_shift[i]+axe->shift[i];
    }

    // Store kinetic variables.
    // Remove center of mass from inertia tensor
    axe->mass=M_part;
    double mr2=Scalar_product_3_d(mr_part, mr_part)/axe->mass;
    for(i=0; i<3; i++){
      axe->MR[i]=mr_part[i];
      for(j=0; j<3; j++){
	axe->I[i][j]=inertia_part[i][j]-mr_part[i]*mr_part[j]/axe->mass;
      }
      axe->I[i][i]+=mr2;
    }

    // New axe
    axe--;
  }

  if(DEBUG){
    double *v, q;
    printf("Axis res local_rot local_shift global_rot global_shift");
    printf(" first_ref first_rotable_atom type\n");
    for(k=0; k<N_axes; k++){
      axe=axes+k;
      printf("%3d %3d ", k, axe->bond->atom->res);
      v=axe->local_rot;
      q=Scalar_product_3_d(v,v); printf(" %.3f ", sqrt(q));
      v=axe->local_shift;
      q=Scalar_product_3_d(v,v); printf("%4.1f ", sqrt(q));
      v=axe->global_rot;
      q=Scalar_product_3_d(v,v); printf(" %.3f ", sqrt(q));
      v=axe->global_shift;
      q=Scalar_product_3_d(v,v); printf("%4.1f ", sqrt(q));
      printf("%3d %3d %c\n",axe->first_kin,axe->first_atom,axe->type);
    }
  }

  // Cleaning
  Empty_matrix_d(corr_sum, 3);
  Empty_matrix_d(inertia_part, 3);
  Empty_matrix_d(inertia_tot, 3);
  Empty_matrix_d(L_mat, 3);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x198, %rsp            # imm = 0x198
movq %rdx, 0x78(%rsp)
movl %esi, %ebp
movq %rdi, %rbx
movl $0x3, %edi
movl $0x3, %esi
callq 0x49804
movq %rax, %r14
xorpd %xmm0, %xmm0
movapd %xmm0, 0xa0(%rsp)
movq $0x0, 0xb0(%rsp)
movslq 0x1d0(%rsp), %rax
testq %rax, %rax
jle 0x21299
movq 0x1e0(%rsp), %rcx
movq 0x1f8(%rsp), %rdx
xorpd %xmm0, %xmm0
xorl %esi, %esi
movslq (%rdx,%rsi,4), %rdi
imulq $0x70, %rdi, %rdi
addq 0x78(%rsp), %rdi
xorps %xmm1, %xmm1
cvtss2sd (%rcx,%rsi,4), %xmm1
xorl %r8d, %r8d
movsd (%rdi,%r8,8), %xmm2
mulsd %xmm1, %xmm2
cvtsd2ss %xmm2, %xmm2
cvtss2sd %xmm2, %xmm2
movsd 0xa0(%rsp,%r8,8), %xmm3
addsd %xmm2, %xmm3
movsd %xmm3, 0xa0(%rsp,%r8,8)
movq (%r14,%r8,8), %r9
movq %r8, %r10
movsd (%rdi,%r10,8), %xmm3
mulsd %xmm2, %xmm3
addsd (%r9,%r10,8), %xmm3
movsd %xmm3, (%r9,%r10,8)
incq %r10
cmpq $0x3, %r10
jne 0x21258
incq %r8
cmpq $0x3, %r8
jne 0x21227
addsd %xmm1, %xmm0
incq %rsi
cmpq %rax, %rsi
jne 0x2120f
cvtsd2ss %xmm0, %xmm0
xorps %xmm1, %xmm1
cvtss2sd %xmm0, %xmm1
jmp 0x2129c
xorps %xmm1, %xmm1
movq %rbx, 0x60(%rsp)
xorl %eax, %eax
movsd 0xa0(%rsp,%rax,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x120(%rsp,%rax,8)
incq %rax
cmpq $0x3, %rax
jne 0x212a3
movsd %xmm1, 0x50(%rsp)
cmpl $0x0, 0x47bdd(%rip)     # 0x68eac
je 0x21322
leaq 0x37454(%rip), %rdi     # 0x5872c
xorl %ebx, %ebx
xorl %eax, %eax
callq 0x20f0
leaq 0x37455(%rip), %r15     # 0x5873d
movsd 0x120(%rsp,%rbx,8), %xmm0
movq %r15, %rdi
movb $0x1, %al
callq 0x20f0
incq %rbx
cmpq $0x3, %rbx
jne 0x212e8
leaq 0x37439(%rip), %rdi     # 0x58744
movsd 0x50(%rsp), %xmm0
movb $0x1, %al
callq 0x20f0
movl $0x0, 0x47b8a(%rip)     # 0x68eac
movl $0x3, %edi
movl $0x3, %esi
callq 0x49804
movq %rax, %rbx
leaq 0xa0(%rsp), %rdx
movq %rax, %rdi
movq %r14, %rsi
movsd 0x50(%rsp), %xmm0
callq 0x226ac
movl $0x3, %edi
movl $0x3, %esi
callq 0x49804
movq %rax, 0x70(%rsp)
movq %rax, %rdi
movq %rbx, 0xe0(%rsp)
movq %rbx, %rsi
movl $0x3, %edx
callq 0x4987c
movl $0x3, %edi
movl $0x3, %esi
callq 0x49804
movq %rax, %r13
xorl %eax, %eax
xorpd %xmm0, %xmm0
xorl %ecx, %ecx
movq %rax, 0x80(%rsp,%rcx,8)
movq (%r14,%rcx,8), %rdx
movq %rax, 0x10(%rdx)
movupd %xmm0, (%rdx)
incq %rcx
cmpq $0x3, %rcx
jne 0x21393
movl %ebp, %eax
movq %rax, 0x68(%rsp)
movl %ebp, 0x44(%rsp)
testl %ebp, %ebp
jle 0x21832
imulq $0x158, 0x68(%rsp), %rax # imm = 0x158
movq 0x60(%rsp), %rcx
leaq (%rcx,%rax), %r12
leaq (%rcx,%rax), %r8
addq $-0x48, %r8
movl $0xffffffff, 0x48(%rsp) # imm = 0xFFFFFFFF
xorl %esi, %esi
leaq 0x80(%rsp), %r15
leaq 0xc0(%rsp), %rbp
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
movl 0x44(%rsp), %eax
movl %eax, %edi
xorl %ebx, %ebx
movl -0x13c(%r12), %eax
cmpl 0x48(%rsp), %eax
je 0x2143e
xorl %ecx, %ecx
movq %rsi, 0x80(%rsp,%rcx,8)
movq (%r14,%rcx,8), %rdx
movq %rsi, 0x10(%rdx)
movupd %xmm0, (%rdx)
incq %rcx
cmpq $0x3, %rcx
jne 0x21417
xorpd %xmm1, %xmm1
movl %eax, 0x48(%rsp)
movl %eax, %ebx
movq %r8, 0xf8(%rsp)
movsd %xmm1, 0x38(%rsp)
movq %rdi, 0xf0(%rsp)
leal -0x1(%rdi), %eax
movl %eax, 0x4c(%rsp)
movl -0x140(%r12), %ecx
leaq 0x1d0(%rsp), %rax
movups (%rax), %xmm0
movups 0x10(%rax), %xmm1
movups 0x20(%rax), %xmm2
movups %xmm2, 0x20(%rsp)
movups %xmm1, 0x10(%rsp)
movups %xmm0, (%rsp)
movq %r15, %rdi
movq %r14, %rsi
movq 0x78(%rsp), %rdx
movl %ebx, %r8d
callq 0x22948
cvtss2sd %xmm0, %xmm0
addsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x38(%rsp)
movq %r13, %rdi
movq %r14, %rsi
movq %r15, %rdx
callq 0x226ac
movsd 0x38(%rsp), %xmm1
movl -0x140(%r12), %ebx
ucomisd 0x2fcad(%rip), %xmm1    # 0x51178
jne 0x21579
jp 0x21579
leaq 0x37271(%rip), %rdi     # 0x5874f
movl 0x4c(%rsp), %ebp
movl %ebp, %esi
xorl %eax, %eax
callq 0x20f0
movl -0x140(%r12), %esi
movl -0x13c(%r12), %edx
leaq 0x37281(%rip), %rdi     # 0x58783
xorl %eax, %eax
callq 0x20f0
movl -0x148(%r12), %esi
movl -0x144(%r12), %edx
leaq 0x3727d(%rip), %rdi     # 0x5879d
xorl %eax, %eax
callq 0x20f0
movl -0x138(%r12), %esi
movl -0x134(%r12), %edx
leaq 0x37273(%rip), %rdi     # 0x587b1
xorl %eax, %eax
callq 0x20f0
movl -0x130(%r12), %esi
testl %esi, %esi
js 0x21561
leaq 0x3726c(%rip), %rdi     # 0x587c4
movl %ebp, %edx
xorl %eax, %eax
callq 0x20f0
movl $0xa, %edi
callq 0x2060
movsd 0x38(%rsp), %xmm1
leaq 0xc0(%rsp), %rbp
leaq -0x158(%r12), %rax
movq %rax, 0xe8(%rsp)
decl %ebx
xorl %eax, %eax
movsd 0xa0(%rsp,%rax,8), %xmm0
mulsd %xmm1, %xmm0
subsd 0x80(%rsp,%rax,8), %xmm0
movsd %xmm0, 0x140(%rsp,%rax,8)
incq %rax
cmpq $0x3, %rax
jne 0x2158d
leaq -0x110(%r12), %rsi
movq %rbp, %rdi
movq %r15, %rdx
callq 0x103bb
movsd 0x38(%rsp), %xmm1
xorl %eax, %eax
movsd 0xc0(%rsp,%rax,8), %xmm0
divsd %xmm1, %xmm0
addsd -0xf8(%r12,%rax,8), %xmm0
movsd %xmm0, 0xc0(%rsp,%rax,8)
incq %rax
cmpq $0x3, %rax
jne 0x215d0
xorl %eax, %eax
movsd 0x120(%rsp,%rax,8), %xmm0
mulsd %xmm1, %xmm0
subsd 0x80(%rsp,%rax,8), %xmm0
movsd %xmm0, 0x140(%rsp,%rax,8)
incq %rax
cmpq $0x3, %rax
jne 0x215fb
leaq 0x100(%rsp), %rdi
leaq 0x140(%rsp), %rsi
movq %rbp, %rdx
callq 0x103bb
xorl %eax, %eax
movq (%r13,%rax,8), %rcx
movsd 0x100(%rsp,%rax,8), %xmm0
xorl %edx, %edx
movsd (%rcx,%rdx,8), %xmm1
mulsd -0x110(%r12,%rdx,8), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x100(%rsp,%rax,8)
incq %rdx
cmpq $0x3, %rdx
jne 0x2164d
incq %rax
cmpq $0x3, %rax
jne 0x2163d
movl %ebx, 0x5c(%rsp)
leaq 0x180(%rsp), %rbx
movq %rbx, %rdi
movq 0x70(%rsp), %r15
movq %r15, %rsi
leaq 0x100(%rsp), %rdx
movl $0x3, %ecx
callq 0x49a2e
leaq -0xb0(%r12), %rbp
movq %rbp, %rdi
movq %r15, %rsi
movq %rbx, %rdx
movl $0x3, %ecx
callq 0x49a86
leaq 0x160(%rsp), %rdi
movq %rbp, %rsi
leaq 0x120(%rsp), %rdx
callq 0x103bb
xorl %eax, %eax
movsd 0x50(%rsp), %xmm1
movapd 0x2f9a9(%rip), %xmm2    # 0x51090
movsd 0x38(%rsp), %xmm3
movsd 0xc0(%rsp,%rax,8), %xmm0
xorpd %xmm2, %xmm0
mulsd %xmm3, %xmm0
divsd %xmm1, %xmm0
subsd 0x160(%rsp,%rax,8), %xmm0
movsd %xmm0, -0xc8(%r12,%rax,8)
incq %rax
cmpq $0x3, %rax
jne 0x216ed
movq $-0x10, %rax
leaq 0x80(%rsp), %r15
movl 0x5c(%rsp), %ebx
movsd -0x30(%r12,%rax,8), %xmm0
addsd -0x90(%r12,%rax,8), %xmm0
movsd -0x48(%r12,%rax,8), %xmm1
movsd %xmm0, (%r12,%rax,8)
addsd -0x78(%r12,%rax,8), %xmm1
movsd %xmm1, -0x18(%r12,%rax,8)
incq %rax
cmpq $-0xd, %rax
jne 0x21731
movsd %xmm3, -0x68(%r12)
movq %r15, %rdi
movq %r15, %rsi
callq 0x10433
divsd -0x68(%r12), %xmm0
leaq -0x48(%r12), %rax
movq 0xf8(%rsp), %r8
movq %r8, %rcx
xorl %edx, %edx
leaq 0xc0(%rsp), %rbp
movl 0x4c(%rsp), %r9d
movsd 0x80(%rsp,%rdx,8), %xmm1
movsd %xmm1, -0x60(%r12,%rdx,8)
movq (%r13,%rdx,8), %rsi
xorl %edi, %edi
movsd (%rsi,%rdi,8), %xmm2
movsd 0x80(%rsp,%rdi,8), %xmm3
mulsd %xmm1, %xmm3
divsd -0x68(%r12), %xmm3
subsd %xmm3, %xmm2
movsd %xmm2, (%rcx,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x217b5
leaq (%rdx,%rdx,2), %rsi
leaq (%rax,%rsi,8), %rsi
movsd (%rsi,%rdx,8), %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, (%rsi,%rdx,8)
incq %rdx
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x2179e
addq $-0x158, %r8            # imm = 0xFEA8
movq 0xe8(%rsp), %r12
cmpl $0x1, 0xf0(%rsp)
movl %r9d, %edi
movl $0x0, %esi
xorpd %xmm0, %xmm0
movsd 0x38(%rsp), %xmm1
jg 0x21407
leaq 0x57bb3(%rip), %rax     # 0x793ec
cmpl $0x0, (%rax)
movq 0x60(%rsp), %rbx
je 0x2197d
leaq 0x36f84(%rip), %rdi     # 0x587d2
xorl %eax, %eax
callq 0x20f0
leaq 0x37169(%rip), %rdi     # 0x589c5
callq 0x2090
cmpl $0x0, 0x44(%rsp)
jle 0x2197d
addq $0x90, %rbx
leaq 0x36f98(%rip), %r12     # 0x58812
leaq 0x36f98(%rip), %rbp     # 0x58819
xorl %r15d, %r15d
movq -0x88(%rbx), %rax
movq (%rax), %rax
movl 0x64(%rax), %edx
leaq 0x36f71(%rip), %rdi     # 0x58809
movl %r15d, %esi
xorl %eax, %eax
callq 0x20f0
leaq 0x48(%rbx), %rdi
movq %rdi, %rsi
callq 0x10433
ucomisd 0x2f8c2(%rip), %xmm0    # 0x51178
jb 0x218be
sqrtsd %xmm0, %xmm0
jmp 0x218c3
callq 0x22a0
movq %r12, %rdi
movb $0x1, %al
callq 0x20f0
leaq 0x30(%rbx), %rdi
movq %rdi, %rsi
callq 0x10433
ucomisd 0x2f897(%rip), %xmm0    # 0x51178
jb 0x218e9
sqrtsd %xmm0, %xmm0
jmp 0x218ee
callq 0x22a0
movq %rbp, %rdi
movb $0x1, %al
callq 0x20f0
leaq 0x18(%rbx), %rdi
movq %rdi, %rsi
callq 0x10433
ucomisd 0x2f86c(%rip), %xmm0    # 0x51178
jb 0x21914
sqrtsd %xmm0, %xmm0
jmp 0x21919
callq 0x22a0
movq %r12, %rdi
movb $0x1, %al
callq 0x20f0
movq %rbx, %rdi
movq %rbx, %rsi
callq 0x10433
ucomisd 0x2f842(%rip), %xmm0    # 0x51178
jb 0x2193e
sqrtsd %xmm0, %xmm0
jmp 0x21943
callq 0x22a0
movq %rbp, %rdi
movb $0x1, %al
callq 0x20f0
movl -0x80(%rbx), %edx
movl -0x78(%rbx), %esi
movsbl -0x90(%rbx), %ecx
leaq 0x36ebf(%rip), %rdi     # 0x58820
xorl %eax, %eax
callq 0x20f0
incq %r15
addq $0x158, %rbx            # imm = 0x158
cmpq %r15, 0x68(%rsp)
jne 0x21884
movq %r14, %rdi
movl $0x3, %esi
callq 0x496ae
movq %r13, %rdi
movl $0x3, %esi
callq 0x496ae
movq 0xe0(%rsp), %rdi
movl $0x3, %esi
callq 0x496ae
movq 0x70(%rsp), %rdi
movl $0x3, %esi
callq 0x496ae
addq $0x198, %rsp            # imm = 0x198
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq