/**
 * @file    transformations.c
 * @brief   Transformations back and forth between different coordinate systems.
 * @author  Hanno Rein <hanno@hanno-rein.de>
 * @details This file collects all the transformations used by the different integrators
 * between different coordinate systems.
 *
 * @section     LICENSE
 * Copyright (c) 2017 Hanno Rein, Dan Tamayo.
 *
 * This file is part of rebound.
 *
 * rebound is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * rebound is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with rebound.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "transformations.h"
#include "rebound.h"

/******************************
 * Jacobi */

void reb_particles_transform_inertial_to_jacobi_posvel(const struct reb_particle* const particles, struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta = p_mass[0].m;
    double s_x = eta * particles[0].x;
    double s_y = eta * particles[0].y;
    double s_z = eta * particles[0].z;
    double s_vx = eta * particles[0].vx;
    double s_vy = eta * particles[0].vy;
    double s_vz = eta * particles[0].vz;
    for (unsigned int i=1;i<N_active;i++){
        const double ei = 1./eta;
        const struct reb_particle pi = particles[i];
        eta += p_mass[i].m;
        const double pme = eta*ei;
        p_j[i].m = pi.m;
        p_j[i].x = pi.x - s_x*ei;
        p_j[i].y = pi.y - s_y*ei;
        p_j[i].z = pi.z - s_z*ei;
        p_j[i].vx = pi.vx - s_vx*ei;
        p_j[i].vy = pi.vy - s_vy*ei;
        p_j[i].vz = pi.vz - s_vz*ei;
        s_x  = s_x  * pme + p_mass[i].m*p_j[i].x ;
        s_y  = s_y  * pme + p_mass[i].m*p_j[i].y ;
        s_z  = s_z  * pme + p_mass[i].m*p_j[i].z ;
        s_vx = s_vx * pme + p_mass[i].m*p_j[i].vx;
        s_vy = s_vy * pme + p_mass[i].m*p_j[i].vy;
        s_vz = s_vz * pme + p_mass[i].m*p_j[i].vz;
    }
    const double ei = 1./eta;
    for (unsigned int i=N_active;i<N;i++){
        const struct reb_particle pi = particles[i];
        p_j[i].m = pi.m;
        p_j[i].x = pi.x - s_x*ei;
        p_j[i].y = pi.y - s_y*ei;
        p_j[i].z = pi.z - s_z*ei;
        p_j[i].vx = pi.vx - s_vx*ei;
        p_j[i].vy = pi.vy - s_vy*ei;
        p_j[i].vz = pi.vz - s_vz*ei;
    }
    const double Mtotal  = eta;
    const double Mtotali = 1./Mtotal;
    p_j[0].m = Mtotal;
    p_j[0].x = s_x * Mtotali;
    p_j[0].y = s_y * Mtotali;
    p_j[0].z = s_z * Mtotali;
    p_j[0].vx = s_vx * Mtotali;
    p_j[0].vy = s_vy * Mtotali;
    p_j[0].vz = s_vz * Mtotali;
}

void reb_particles_transform_inertial_to_jacobi_posvelacc(const struct reb_particle* const particles, struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta = p_mass[0].m;
    double s_x = eta * particles[0].x;
    double s_y = eta * particles[0].y;
    double s_z = eta * particles[0].z;
    double s_vx = eta * particles[0].vx;
    double s_vy = eta * particles[0].vy;
    double s_vz = eta * particles[0].vz;
    double s_ax = eta * particles[0].ax;
    double s_ay = eta * particles[0].ay;
    double s_az = eta * particles[0].az;
    for (unsigned int i=1;i<N_active;i++){
        const double ei = 1./eta;
        const struct reb_particle pi = particles[i];
        eta += p_mass[i].m;
        const double pme = eta*ei;
        p_j[i].m = pi.m;
        p_j[i].x = pi.x - s_x*ei;
        p_j[i].y = pi.y - s_y*ei;
        p_j[i].z = pi.z - s_z*ei;
        p_j[i].vx = pi.vx - s_vx*ei;
        p_j[i].vy = pi.vy - s_vy*ei;
        p_j[i].vz = pi.vz - s_vz*ei;
        p_j[i].ax = pi.ax - s_ax*ei;
        p_j[i].ay = pi.ay - s_ay*ei;
        p_j[i].az = pi.az - s_az*ei;
        s_x  = s_x  * pme + p_mass[i].m*p_j[i].x ;
        s_y  = s_y  * pme + p_mass[i].m*p_j[i].y ;
        s_z  = s_z  * pme + p_mass[i].m*p_j[i].z ;
        s_vx = s_vx * pme + p_mass[i].m*p_j[i].vx;
        s_vy = s_vy * pme + p_mass[i].m*p_j[i].vy;
        s_vz = s_vz * pme + p_mass[i].m*p_j[i].vz;
        s_ax = s_ax * pme + p_mass[i].m*p_j[i].ax;
        s_ay = s_ay * pme + p_mass[i].m*p_j[i].ay;
        s_az = s_az * pme + p_mass[i].m*p_j[i].az;
    }
    const double ei = 1./eta;
    for (unsigned int i=N_active;i<N;i++){
        const struct reb_particle pi = particles[i];
        p_j[i].m = pi.m;
        p_j[i].x = pi.x - s_x*ei;
        p_j[i].y = pi.y - s_y*ei;
        p_j[i].z = pi.z - s_z*ei;
        p_j[i].vx = pi.vx - s_vx*ei;
        p_j[i].vy = pi.vy - s_vy*ei;
        p_j[i].vz = pi.vz - s_vz*ei;
        p_j[i].ax = pi.ax - s_ax*ei;
        p_j[i].ay = pi.ay - s_ay*ei;
        p_j[i].az = pi.az - s_az*ei;
    }
    const double Mtotal  = eta;
    const double Mtotali = 1./Mtotal;
    p_j[0].m = Mtotal;
    p_j[0].x = s_x * Mtotali;
    p_j[0].y = s_y * Mtotali;
    p_j[0].z = s_z * Mtotali;
    p_j[0].vx = s_vx * Mtotali;
    p_j[0].vy = s_vy * Mtotali;
    p_j[0].vz = s_vz * Mtotali;
    p_j[0].ax = s_ax * Mtotali;
    p_j[0].ay = s_ay * Mtotali;
    p_j[0].az = s_az * Mtotali;
}

void reb_particles_transform_inertial_to_jacobi_acc(const struct reb_particle* const particles, struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta = p_mass[0].m;
    double s_ax = eta * particles[0].ax;
    double s_ay = eta * particles[0].ay;
    double s_az = eta * particles[0].az;
    for (unsigned int i=1;i<N_active;i++){
        const double ei = 1./eta;
        const struct reb_particle pi = particles[i];
        eta += p_mass[i].m;
        const double pme = eta*ei;
        p_j[i].ax = pi.ax - s_ax*ei;
        p_j[i].ay = pi.ay - s_ay*ei;
        p_j[i].az = pi.az - s_az*ei;
        s_ax = s_ax * pme + p_mass[i].m*p_j[i].ax;
        s_ay = s_ay * pme + p_mass[i].m*p_j[i].ay;
        s_az = s_az * pme + p_mass[i].m*p_j[i].az;
    }
    const double ei = 1./eta;
    for (unsigned int i=N_active;i<N;i++){
        const struct reb_particle pi = particles[i];
        p_j[i].ax = pi.ax - s_ax*ei;
        p_j[i].ay = pi.ay - s_ay*ei;
        p_j[i].az = pi.az - s_az*ei;
    }
    const double Mtotal  = eta;
    const double Mtotali = 1./Mtotal;
    p_j[0].ax = s_ax * Mtotali;
    p_j[0].ay = s_ay * Mtotali;
    p_j[0].az = s_az * Mtotali;
}

void reb_particles_transform_jacobi_to_inertial_posvel(struct reb_particle* const particles, const struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta  = p_j[0].m;
    double s_x  = p_j[0].x  * eta;
    double s_y  = p_j[0].y  * eta;
    double s_z  = p_j[0].z  * eta;
    double s_vx = p_j[0].vx * eta;
    double s_vy = p_j[0].vy * eta;
    double s_vz = p_j[0].vz * eta;
    for (unsigned int i=N-1;i>=N_active;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        particles[i].x  = pji.x  + s_x  * ei;
        particles[i].y  = pji.y  + s_y  * ei;
        particles[i].z  = pji.z  + s_z  * ei;
        particles[i].vx = pji.vx + s_vx * ei;
        particles[i].vy = pji.vy + s_vy * ei;
        particles[i].vz = pji.vz + s_vz * ei;
    }
    for (unsigned int i=N_active-1;i>0;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        s_x  = (s_x  - p_mass[i].m * pji.x ) * ei;
        s_y  = (s_y  - p_mass[i].m * pji.y ) * ei;
        s_z  = (s_z  - p_mass[i].m * pji.z ) * ei;
        s_vx = (s_vx - p_mass[i].m * pji.vx) * ei;
        s_vy = (s_vy - p_mass[i].m * pji.vy) * ei;
        s_vz = (s_vz - p_mass[i].m * pji.vz) * ei;
        particles[i].x  = pji.x  + s_x ;
        particles[i].y  = pji.y  + s_y ;
        particles[i].z  = pji.z  + s_z ;
        particles[i].vx = pji.vx + s_vx;
        particles[i].vy = pji.vy + s_vy;
        particles[i].vz = pji.vz + s_vz;
        eta -= p_mass[i].m;
        s_x  *= eta;
        s_y  *= eta;
        s_z  *= eta;
        s_vx *= eta;
        s_vy *= eta;
        s_vz *= eta;
    }
    const double mi = 1./eta;
    particles[0].x  = s_x  * mi;
    particles[0].y  = s_y  * mi;
    particles[0].z  = s_z  * mi;
    particles[0].vx = s_vx * mi;
    particles[0].vy = s_vy * mi;
    particles[0].vz = s_vz * mi;
}

void reb_particles_transform_jacobi_to_inertial_pos(struct reb_particle* const particles, const struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta  = p_j[0].m;
    double s_x  = p_j[0].x  * eta;
    double s_y  = p_j[0].y  * eta;
    double s_z  = p_j[0].z  * eta;
    for (unsigned int i=N-1;i>=N_active;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        particles[i].x  = pji.x  + s_x*ei ;
        particles[i].y  = pji.y  + s_y*ei ;
        particles[i].z  = pji.z  + s_z*ei ;
    }
    for (unsigned int i=N_active-1;i>0;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        s_x  = (s_x  - p_mass[i].m * pji.x ) * ei;
        s_y  = (s_y  - p_mass[i].m * pji.y ) * ei;
        s_z  = (s_z  - p_mass[i].m * pji.z ) * ei;
        particles[i].x  = pji.x  + s_x ;
        particles[i].y  = pji.y  + s_y ;
        particles[i].z  = pji.z  + s_z ;
        eta -= p_mass[i].m;
        s_x  *= eta;
        s_y  *= eta;
        s_z  *= eta;
    }
    const double mi = 1./eta;
    particles[0].x  = s_x  * mi;
    particles[0].y  = s_y  * mi;
    particles[0].z  = s_z  * mi;
}

void reb_particles_transform_jacobi_to_inertial_acc(struct reb_particle* const particles, const struct reb_particle* const p_j, const struct reb_particle* const p_mass, const unsigned int N, const unsigned int N_active){
    double eta  = p_j[0].m;
    double s_ax  = p_j[0].ax  * eta;
    double s_ay  = p_j[0].ay  * eta;
    double s_az  = p_j[0].az  * eta;
    for (unsigned int i=N-1;i>=N_active;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        particles[i].ax  = pji.ax  + s_ax * ei;
        particles[i].ay  = pji.ay  + s_ay * ei;
        particles[i].az  = pji.az  + s_az * ei;
    }
    for (unsigned int i=N_active-1;i>0;i--){
        const struct reb_particle pji = p_j[i];
        const double ei = 1./eta;
        s_ax  = (s_ax  - p_mass[i].m * pji.ax ) * ei;
        s_ay  = (s_ay  - p_mass[i].m * pji.ay ) * ei;
        s_az  = (s_az  - p_mass[i].m * pji.az ) * ei;
        particles[i].ax  = pji.ax  + s_ax ;
        particles[i].ay  = pji.ay  + s_ay ;
        particles[i].az  = pji.az  + s_az ;
        eta -= p_mass[i].m;
        s_ax  *= eta;
        s_ay  *= eta;
        s_az  *= eta;
    }
    const double mi = 1./eta;
    particles[0].ax  = s_ax  * mi;
    particles[0].ay  = s_ay  * mi;
    particles[0].az  = s_az  * mi;
}

/******************************
 * WHDS (Hernandez)           */

void reb_particles_transform_inertial_to_whds_posvel(const struct reb_particle* const particles, struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    double x0  = 0.;
    double y0  = 0.;
    double z0  = 0.;
    double vx0 = 0.;
    double vy0 = 0.;
    double vz0 = 0.;
    double m0  = 0.;
#pragma omp parallel for reduction(+:x0) reduction(+:y0) reduction(+:z0) reduction(+:vx0) reduction(+:vy0) reduction(+:vz0) reduction(+:m0)
    for (unsigned int i=0;i<N_active;i++){
        double m = particles[i].m;
        x0  += particles[i].x *m;
        y0  += particles[i].y *m;
        z0  += particles[i].z *m;
        vx0 += particles[i].vx*m;
        vy0 += particles[i].vy*m;
        vz0 += particles[i].vz*m;
        m0  += m;
    }
    p_h[0].x  = x0/m0;
    p_h[0].y  = y0/m0;
    p_h[0].z  = z0/m0;
    p_h[0].vx = vx0/m0;
    p_h[0].vy = vy0/m0;
    p_h[0].vz = vz0/m0;
    p_h[0].m = m0;

    m0 = particles[0].m;
#pragma omp parallel for
    for (unsigned int i=1;i<N_active;i++){
        p_h[i].x  = particles[i].x  - particles[0].x ;
        p_h[i].y  = particles[i].y  - particles[0].y ;
        p_h[i].z  = particles[i].z  - particles[0].z ;
        const double mi = particles[i].m;
        double mf = (m0+mi) / m0;
        p_h[i].vx = mf*(particles[i].vx - p_h[0].vx);
        p_h[i].vy = mf*(particles[i].vy - p_h[0].vy);
        p_h[i].vz = mf*(particles[i].vz - p_h[0].vz);
        p_h[i].m  = mi;
    }
#pragma omp parallel for
    for (unsigned int i=N_active;i<N;i++){
        p_h[i].x  = particles[i].x  - particles[0].x ;
        p_h[i].y  = particles[i].y  - particles[0].y ;
        p_h[i].z  = particles[i].z  - particles[0].z ;
        p_h[i].vx = particles[i].vx - p_h[0].vx;
        p_h[i].vy = particles[i].vy - p_h[0].vy;
        p_h[i].vz = particles[i].vz - p_h[0].vz;
        p_h[i].m  = particles[i].m;
    }
}

void reb_particles_transform_whds_to_inertial_pos(struct reb_particle* const particles, const struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    // Same as in heliocentric case.
    reb_particles_transform_democraticheliocentric_to_inertial_pos(particles, p_h, N, N_active);
}

void reb_particles_transform_whds_to_inertial_posvel(struct reb_particle* const particles, const struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    reb_particles_transform_whds_to_inertial_pos(particles,p_h,N, N_active);
    const double m0 = particles[0].m;
#pragma omp parallel for
    for (unsigned int i=1;i<N_active;i++){
        const double mi = particles[i].m;
        double mf = (m0+mi) / m0;
        particles[i].vx = p_h[i].vx/mf+p_h[0].vx;
        particles[i].vy = p_h[i].vy/mf+p_h[0].vy;
        particles[i].vz = p_h[i].vz/mf+p_h[0].vz;
    }
#pragma omp parallel for
    for (unsigned int i=N_active;i<N;i++){
        particles[i].vx = p_h[i].vx+p_h[0].vx;
        particles[i].vy = p_h[i].vy+p_h[0].vy;
        particles[i].vz = p_h[i].vz+p_h[0].vz;
    }
    double vx0  = 0.;
    double vy0  = 0.;
    double vz0  = 0.;
#pragma omp parallel for reduction(+:vx0) reduction(+:vy0) reduction(+:vz0) 
    for (unsigned int i=1;i<N_active;i++){
        double m = particles[i].m;
        vx0 += p_h[i].vx*m/(m0+m);
        vy0 += p_h[i].vy*m/(m0+m);
        vz0 += p_h[i].vz*m/(m0+m);
    }
    particles[0].vx = p_h[0].vx -vx0;
    particles[0].vy = p_h[0].vy -vy0;
    particles[0].vz = p_h[0].vz -vz0;
}

/******************************
 * Democratic heliocentric.   *
 * Duncan, Levison & Lee 1998 */

void reb_particles_transform_inertial_to_democraticheliocentric_posvel(const struct reb_particle* const particles, struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    double x0  = 0.;
    double y0  = 0.;
    double z0  = 0.;
    double vx0 = 0.;
    double vy0 = 0.;
    double vz0 = 0.;
    double m0  = 0.;
#pragma omp parallel for reduction(+:x0) reduction(+:y0) reduction(+:z0) reduction(+:vx0) reduction(+:vy0) reduction(+:vz0) reduction(+:m0)
    for (unsigned int i=0;i<N_active;i++){
        double m = particles[i].m;
        x0  += particles[i].x *m;
        y0  += particles[i].y *m;
        z0  += particles[i].z *m;
        vx0 += particles[i].vx*m;
        vy0 += particles[i].vy*m;
        vz0 += particles[i].vz*m;
        m0  += m;
    }
    p_h[0].x  = x0/m0;
    p_h[0].y  = y0/m0;
    p_h[0].z  = z0/m0;
    p_h[0].vx = vx0/m0;
    p_h[0].vy = vy0/m0;
    p_h[0].vz = vz0/m0;
    p_h[0].m = m0;

#pragma omp parallel for 
    for (unsigned int i=1;i<N;i++){
        p_h[i].x  = particles[i].x  - particles[0].x ;
        p_h[i].y  = particles[i].y  - particles[0].y ;
        p_h[i].z  = particles[i].z  - particles[0].z ;
        p_h[i].vx = particles[i].vx - p_h[0].vx;
        p_h[i].vy = particles[i].vy - p_h[0].vy;
        p_h[i].vz = particles[i].vz - p_h[0].vz;
        p_h[i].m  = particles[i].m;
    }
}

void reb_particles_transform_democraticheliocentric_to_inertial_pos(struct reb_particle* const particles, const struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    const double mtot = p_h[0].m;
    double x0  = 0.;
    double y0  = 0.;
    double z0  = 0.;
#pragma omp parallel for reduction(+:x0) reduction(+:y0) reduction(+:z0) 
    for (unsigned int i=1;i<N_active;i++){
        double m = p_h[i].m;
        x0 += p_h[i].x*m/mtot;
        y0 += p_h[i].y*m/mtot;
        z0 += p_h[i].z*m/mtot;
        particles[i].m = m; // in case of merger/mass change
    }
    particles[0].x  = p_h[0].x - x0;
    particles[0].y  = p_h[0].y - y0;
    particles[0].z  = p_h[0].z - z0;
#pragma omp parallel for 
    for (unsigned int i=1;i<N;i++){
        particles[i].x = p_h[i].x+particles[0].x;
        particles[i].y = p_h[i].y+particles[0].y;
        particles[i].z = p_h[i].z+particles[0].z;
    }
}

void reb_particles_transform_democraticheliocentric_to_inertial_posvel(struct reb_particle* const particles, const struct reb_particle* const p_h, const unsigned int N, const unsigned int N_active){
    reb_particles_transform_democraticheliocentric_to_inertial_pos(particles,p_h,N,N_active);
    const double m0 = particles[0].m;
#pragma omp parallel for 
    for (unsigned int i=1;i<N;i++){
        particles[i].vx = p_h[i].vx+p_h[0].vx;
        particles[i].vy = p_h[i].vy+p_h[0].vy;
        particles[i].vz = p_h[i].vz+p_h[0].vz;
    }
    double vx0  = 0.;
    double vy0  = 0.;
    double vz0  = 0.;
#pragma omp parallel for reduction(+:vx0) reduction(+:vy0) reduction(+:vz0) 
    for (unsigned int i=1;i<N_active;i++){
        double m = particles[i].m;
        vx0 += p_h[i].vx*m/m0;
        vy0 += p_h[i].vy*m/m0;
        vz0 += p_h[i].vz*m/m0;
    }
    particles[0].vx = p_h[0].vx -vx0;
    particles[0].vy = p_h[0].vy -vy0;
    particles[0].vz = p_h[0].vz -vz0;
}

/***************
 * Barycentric */

void reb_particles_transform_barycentric_to_inertial_pos(struct reb_particle* const particles, const struct reb_particle* const p_b, const unsigned int N, const unsigned int N_active){
    particles[0].x = p_b[0].m*p_b[0].x;
    particles[0].y = p_b[0].m*p_b[0].y;
    particles[0].z = p_b[0].m*p_b[0].z;
    particles[0].m = p_b[0].m;

    double s_x = 0.0;
    double s_y = 0.0;
    double s_z = 0.0;
    double s_m = 0.0;
    for (unsigned int i=1; i<N; i++) {
        particles[i].x = p_b[i].x + p_b[0].x;
        particles[i].y = p_b[i].y + p_b[0].y;
        particles[i].z = p_b[i].z + p_b[0].z;
        if(i < N_active){
            const double m = p_b[i].m;
            particles[i].m = m;
            s_x += particles[i].x*m;
            s_y += particles[i].y*m;
            s_z += particles[i].z*m;
            s_m += m;
        }
    }
    particles[0].x -= s_x;
    particles[0].y -= s_y;
    particles[0].z -= s_z;
    particles[0].m -= s_m;
    const double m0i = 1./ particles[0].m;
    particles[0].x *= m0i;
    particles[0].y *= m0i;
    particles[0].z *= m0i;
}

void reb_particles_transform_barycentric_to_inertial_acc(struct reb_particle* const particles, const struct reb_particle* const p_b, const unsigned int N, const unsigned int N_active){
    particles[0].ax = p_b[0].m*p_b[0].ax;
    particles[0].ay = p_b[0].m*p_b[0].ay;
    particles[0].az = p_b[0].m*p_b[0].az;
    particles[0].m = p_b[0].m;

    double s_ax = 0.0;
    double s_ay = 0.0;
    double s_az = 0.0;
    double s_m = 0.0;
    for (unsigned int i=1; i<N; i++) {
        particles[i].ax = p_b[i].ax + p_b[0].ax;
        particles[i].ay = p_b[i].ay + p_b[0].ay;
        particles[i].az = p_b[i].az + p_b[0].az;
        if(i < N_active){
            const double m = p_b[i].m;
            particles[i].m = m;
            s_ax += particles[i].ax*m;
            s_ay += particles[i].ay*m;
            s_az += particles[i].az*m;
            s_m += m;
        }
    }
    particles[0].ax -= s_ax;
    particles[0].ay -= s_ay;
    particles[0].az -= s_az;
    particles[0].m -= s_m;
    const double m0i = 1./ particles[0].m;
    particles[0].ax *= m0i;
    particles[0].ay *= m0i;
    particles[0].az *= m0i;
} 

void reb_particles_transform_barycentric_to_inertial_posvel(struct reb_particle* const particles, const struct reb_particle* const p_b, const unsigned int N, const unsigned int N_active){
    particles[0].x = p_b[0].m*p_b[0].x;
    particles[0].y = p_b[0].m*p_b[0].y;
    particles[0].z = p_b[0].m*p_b[0].z;
    particles[0].vx = p_b[0].m*p_b[0].vx;
    particles[0].vy = p_b[0].m*p_b[0].vy;
    particles[0].vz = p_b[0].m*p_b[0].vz;
    particles[0].m = p_b[0].m;

    double s_x = 0.0;
    double s_y = 0.0;
    double s_z = 0.0;
    double s_vx = 0.0;
    double s_vy = 0.0;
    double s_vz = 0.0;
    double s_m = 0.0;
    for (unsigned int i=1; i<N; i++) {
        particles[i].x = p_b[i].x + p_b[0].x;
        particles[i].y = p_b[i].y + p_b[0].y;
        particles[i].z = p_b[i].z + p_b[0].z;
        particles[i].vx = p_b[i].vx + p_b[0].vx;
        particles[i].vy = p_b[i].vy + p_b[0].vy;
        particles[i].vz = p_b[i].vz + p_b[0].vz;
        if(i < N_active) {
            const double m = p_b[i].m;
            particles[i].m = m;
            s_x += particles[i].x*m;
            s_y += particles[i].y*m;
            s_z += particles[i].z*m;
            s_vx += particles[i].vx*m;
            s_vy += particles[i].vy*m;
            s_vz += particles[i].vz*m;
            s_m += m;
        }
    }
    particles[0].x -= s_x;
    particles[0].y -= s_y;
    particles[0].z -= s_z;
    particles[0].vx -= s_vx;
    particles[0].vy -= s_vy;
    particles[0].vz -= s_vz;
    particles[0].m -= s_m;
    const double m0i = 1./particles[0].m;
    particles[0].x *= m0i;
    particles[0].y *= m0i;
    particles[0].z *= m0i;
    particles[0].vx *= m0i;
    particles[0].vy *= m0i;
    particles[0].vz *= m0i;
}

void reb_particles_transform_inertial_to_barycentric_posvel(const struct reb_particle* const particles, struct reb_particle* const p_b, const unsigned int N, const unsigned int N_active) {
    p_b[0].x = particles[0].m*particles[0].x;
    p_b[0].y = particles[0].m*particles[0].y;
    p_b[0].z = particles[0].m*particles[0].z;
    p_b[0].vx = particles[0].m*particles[0].vx;
    p_b[0].vy = particles[0].m*particles[0].vy;
    p_b[0].vz = particles[0].m*particles[0].vz;
    p_b[0].m = particles[0].m;

    double s_x = 0.0;
    double s_y = 0.0;
    double s_z = 0.0;
    double s_vx = 0.0;
    double s_vy = 0.0;
    double s_vz = 0.0;
    double s_m = 0.0;
    for (unsigned int i = 1; i < N_active; i++) {
        const double m = particles[i].m;
        p_b[i].m = m;
        s_x += particles[i].x*m;
        s_y += particles[i].y*m;
        s_z += particles[i].z*m;
        s_vx += particles[i].vx*m;
        s_vy += particles[i].vy*m;
        s_vz += particles[i].vz*m;
        s_m += m;
    }
    p_b[0].x += s_x;
    p_b[0].y += s_y;
    p_b[0].z += s_z;
    p_b[0].vx += s_vx;
    p_b[0].vy += s_vy;
    p_b[0].vz += s_vz;
    p_b[0].m += s_m;
    const double mi = 1.0 / p_b[0].m;
    p_b[0].x *= mi;
    p_b[0].y *= mi;
    p_b[0].z *= mi;
    p_b[0].vx *= mi;
    p_b[0].vy *= mi;
    p_b[0].vz *= mi;
    for (unsigned int i = 1;i<N;i++) {
        p_b[i].x = particles[i].x - p_b[0].x;
        p_b[i].y = particles[i].y - p_b[0].y;
        p_b[i].z = particles[i].z - p_b[0].z;
        p_b[i].vx = particles[i].vx - p_b[0].vx;
        p_b[i].vy = particles[i].vy - p_b[0].vy;
        p_b[i].vz = particles[i].vz - p_b[0].vz;
    }
}
