/* ----------------------------------------
 * Origin  : GFI INFORMATIQUE 
 * Project : CDPP Propagation Tool 
 * ----------------------------------------
 * History 
 * Version : 01/05/2015 : SCA : V2.1
 * VERSION : 27/10/2022 : LBE
 * DM-ID : redmine#9229 : 27/10/2022 : added PFSS methods
 * Version : 12/09/2023 : LBE : V2.8
 * DM-ID : Redmine#11197 : 12/09/2023 : 3DShocks improvments
 * Version : 05/08/2024 : LBE : V2.11
 * DM-ID : Redmine#l0763 : 05/08/2024 : PFSS grid lines
 * ----------------------------------------
 */
package fr.tlse.irap.server.frontServices;

import fr.tlse.irap.fieldLine.Cube;
import fr.tlse.irap.fieldLine.FieldLine;
import fr.tlse.irap.fieldLine.FieldLineWrapper;
import fr.tlse.irap.fieldLine.FieldPoint;
import java.util.Date;

import javax.jws.WebService;

import fr.tlse.irap.fieldLine.Footpoints;
import fr.tlse.irap.fieldLine.IFieldLine;
import fr.tlse.irap.pos.Position;
import fr.tlse.irap.pos.PositionWrapper;
import fr.tlse.irap.server.serviceDAO.IAsciiDAO;
import fr.tlse.irap.spaceElement.SpaceElement;
import fr.tlse.irap.spaceElement.SpaceElementWrapper;
import fr.tlse.irap.utils.CoordConverter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import javax.vecmath.Point3f;
import org.apache.log4j.Logger;

@WebService(endpointInterface = "fr.tlse.irap.fieldLine.IFieldLine")
public class FieldLineImpl implements IFieldLine {

    private static final Logger logger = Logger.getLogger(FieldLineImpl.class);
    private IAsciiDAO asciiDAO;

    public FieldLineImpl() {
    }

    public void setAsciiDAO(IAsciiDAO asciiDAO) {
        this.asciiDAO = asciiDAO;
    }

    public IAsciiDAO getAsciiDAO() {
        return asciiDAO;
    }

    //@WebMethod
    @Override
    public Footpoints getEndFieldLineFP(Footpoints startFieldLineFP, Date time, String model, String source) {
        Footpoints footpoints = null;
        try {
            footpoints = new Footpoints(asciiDAO.loadEndFieldLineFP(startFieldLineFP, time, model, source));

        } catch (Exception e) {
            e.printStackTrace();
        }
        return footpoints;
    }

    /**
     * Get projection from parker spiral then PFSS integration from an element
     * position
     *
     * @param pLstSe List of element fo start positions in (HAE/EclipJ2000) in
     * km
     * @param pTime Time used to select PFSS cube
     * @param pSWSpeed Solar wind speed in km.s-1
     * @return List of fieldLines in carrington/IAU_SUN
     */
    //@WebMethod
    @Override
    public FieldLineWrapper getPSPFSSFieldLine(SpaceElementWrapper pSe, Date pTime, double pSWSpeed, double[] pSunAtt) {
        FieldLineWrapper lFieldLines = new FieldLineWrapper();
        final double RSun = 696340;
        final double RSourceFactor = 2.5;
        try {
            //logger.info("Entering PFPFSSFieldLine with " + pSe.getSpaceElements().size() + " elements.");
            Cube lCube = asciiDAO.loadPFSSCube(pTime, "PFSS", "Bestof");
            if (lCube == null) {
                throw new IOException("Cannot find cube for PFSS integration at " + pTime);
            }

            for (SpaceElement lSe : pSe.getSpaceElements()) {
                /**
                 * 1) check if lSe position > 2.5 RS parker spiral till 2.5 <=
                 * new line ending at 2.5. 2) get pfss cube and integrate last
                 * fline point <= field line to 1.0 RS. 3) complete fieldLine.
                 * 4) add fieldLine to list
                 */
                FieldLine lFl = new FieldLine();
                Position lStartPos = lSe.getPosition();
                if (lStartPos.length() > 2.5 * RSun) { // should be always true as all objects are outside 2.5R
                    //get parker spiral

                    final double lSolRot = 25.38;
                    //get rot angle per second
                    final double lAngularSpeed = 2 * Math.PI / (lSolRot * 24 * 60 * 60);
                    final double rSource = RSourceFactor * RSun;

                    double lP[] = CoordConverter.toSpheric(lStartPos.getX(), lStartPos.getY(), lStartPos.getZ());
                    //System.out.println("pointToFollow = " + pointToFollow);
                    double lRange = lSe.getPosition().length();
                    double lRangeMax = lRange;
                    //if max range set on followed target
                    double lPhi = Math.toRadians(lP[0]);
                    double lTheta = Math.toRadians(lP[1]);
                    //System.out.println("lRange = "+lRange+" Phi="+lP[0]+" theta="+lP[1]);
//            //convert thta to angle from Z axis

                    double lPhi0 = lPhi + (lAngularSpeed * (lRange - rSource)) / pSWSpeed;

                    //LOGGER.info("pRadius=" + pRadius + " pSolarwspeed=" + pSolarwspeed + " lAngularSpeed=" + lAngularSpeed);
                    //LOGGER.info("Range=" + lRange + " Phi=" + lPhi + " lPhi0=" + lPhi0);
                    List<FieldPoint> lPts = new ArrayList<FieldPoint>();

                    //System.out.println("Build line " + i + " Phi0=" + Math.toDegrees(lPhi0) + " theta=" + Math.toDegrees(lTheta));
                    //logger.info("Parker spiral point Build "+lSe.getName()+": ("+lStartPos.getX()+"/"+lStartPos.getY()+"/"+lStartPos.getZ()+") - Att:"+pSunAtt[0]+" - "+lP[0]+" lat="+lP[1]+" r="+ lRange);
                    //Build line
                    for (lRange = rSource; lRange < lRangeMax; lRange += RSun) {

                        /**
                         * **** CRAVENS 3D wi th no simplification ****
                         */
                        lPhi = lPhi0 - (lAngularSpeed * (lRange - rSource)) / pSWSpeed;

                        //lRange = pRadius + (pSolarwspeed * (lPhi - lPhi0)) / lAngularSpeed;
                        //logger.info("**********Using sun attitude "+pSunAtt[0]+"/"+pSunAtt[1]);
                        //convert to carrington(IAU) spheric coords then spheric
                        double[] lPt = CoordConverter.toCartesian(Math.toDegrees(lPhi) - pSunAtt[0], Math.toDegrees(lTheta) - pSunAtt[1], lRange);
                        //logger.info("Parker spiral point (Eclip)"+lSe.getName()+": "+(Math.toDegrees(lPhi))+" lat="+ (Math.toDegrees(lTheta))+" r="+ lRange);
                        //logger.info("Parker spiral point (IAU)"+lSe.getName()+": "+(Math.toDegrees(lPhi)-pSunAtt[0])+" lat="+ (Math.toDegrees(lTheta)-pSunAtt[1])+" r="+ lRange);
                        //CoordConverter.rectangularToCarrington(lPt[0], lPt[1], lPt[2], pSunAtt);
                        FieldPoint lFp = new FieldPoint(lPt[0], lPt[1], lPt[2], 0, 0, 0);
                        lPts.add(lFp);
                        //lList.add(lPoint);
                    }
                    //Add all parker spiral line points to field line from start point to surface
                    for (int i = lPts.size() - 1; i >= 0; i--) {
                        lFl.getPoints().add(lPts.get(i));
                    }
                }

                //PFSS start point is lPhi0 or last lFl Point
                FieldPoint lPfssInput = lFl.getPoints().get(lFl.getPoints().size() - 1);
                //set Index where pfss points start
                lFl.setPfssIdx(lFl.getPoints().size() - 1);

                //Launch pffss integration on only one input (current spaceElement) in IAU? ecliptic ?
                //List<double[]> lInputs = new ArrayList<double[]>();
                //lInputs.add(new double[]{lPfssInput.getX(),lPfssInput.getY(),lPfssInput.getZ()});
                double lStepSize = 0.1;
                int lMaxSteps = 500;
                Integer lStepsReturned = 0;
                double[] lInput = new double[]{lPfssInput.getX() / RSun, lPfssInput.getY() / RSun, lPfssInput.getZ() / RSun};
                //List<double[]> lFl = new ArrayList<>();
                List<double[]> lFl1 = traceFieldLine(lCube, lInput, lStepSize, -1, lMaxSteps, lStepsReturned);
                //linesBg.addChild(drawLine(lFl,true));
                for (int i = 1; i < lFl1.size(); i++) {
                    lFl.getPoints().add(new FieldPoint(lFl1.get(i)[0] * RSun, lFl1.get(i)[1] * RSun, lFl1.get(i)[2] * RSun, lFl1.get(i)[3], lFl1.get(i)[4], lFl1.get(i)[5]));
                    /*System.out.println("Found point "+lFl1.get(i)[0]+"/"+lFl1.get(i)[1]+"/"+lFl1.get(i)[2]+" "
                             +" dist="+Math.sqrt(lFl1.get(i)[0]*lFl1.get(i)[0]+lFl1.get(i)[1]*lFl1.get(i)[1]+lFl1.get(i)[2]*lFl1.get(i)[2]));
                     */
                }
                //System.out.println("Found " + lFl1.size() + " values for BACK line.");
                List<double[]> lFl2 = traceFieldLine(lCube, lInput, lStepSize, 1, lMaxSteps, lStepsReturned);
                for (int i = 1; i < lFl2.size(); i++) {
                    lFl.getPoints().add(new FieldPoint(lFl2.get(i)[0] * RSun, lFl2.get(i)[1] * RSun, lFl2.get(i)[2] * RSun, lFl2.get(i)[3], lFl2.get(i)[4], lFl2.get(i)[5]));
                    /*System.out.println("Found point "+lFl2.get(i)[0]+"/"+lFl2.get(i)[1]+"/"+lFl2.get(i)[2]+" "
                             +" dist="+Math.sqrt(lFl2.get(i)[0]*lFl2.get(i)[0]+lFl2.get(i)[1]*lFl2.get(i)[1]+lFl2.get(i)[2]*lFl2.get(i)[2]));
                     */
                }
                //linesBg.addChild(drawLine(lFl,true));
                //System.out.println("Found " + lFl2.size() + " values for FORWARD line.");

                lFieldLines.getFieldLines().add(lFl);

                //=>>>>>>>>>>>>>>>> Reverse points to eclip ?
            }
        } catch (Exception e) {
            e.printStackTrace();
        }
        return lFieldLines;
    }

    @Override
    public FieldLineWrapper getPFSSGridFieldLine(PositionWrapper pStartPts, Date[] pTimes) {
        FieldLineWrapper lFieldLines = new FieldLineWrapper();
        final double RSun = 696340;
        try {

            for (Date lDate : pTimes) {
                //logger.info("Entering PFPFSSFieldLine with " + pSe.getSpaceElements().size() + " elements.");
                Cube lCube = asciiDAO.loadPFSSCube(lDate, "PFSS", "Bestof");
                if (lCube == null) {
                    //throw new IOException("Cannot find cube for PFSS integration at " + pTimes);
                    continue; //search next cube
                }

                for (Position lPos : pStartPts.getPositions()) {
                    FieldLine lFl = new FieldLine();

                    double lStepSize = 0.1;
                    int lMaxSteps = 500;
                    Integer lStepsReturned = 0;
                    double[] lInput = new double[]{lPos.getX(), lPos.getY(), lPos.getZ()};
                    //List<double[]> lFl = new ArrayList<>();
                    List<double[]> lFl1 = traceFieldLine(lCube, lInput, lStepSize, -1, lMaxSteps, lStepsReturned);
                    //linesBg.addChild(drawLine(lFl,true));
                    for (int i = lFl1.size()-1; i >=0 ; i--) {
                        lFl.getPoints().add(new FieldPoint(lFl1.get(i)[0] * RSun, lFl1.get(i)[1] * RSun, lFl1.get(i)[2] * RSun, lFl1.get(i)[3], lFl1.get(i)[4], lFl1.get(i)[5]));
                        /*System.out.println("Found point "+lFl1.get(i)[0]+"/"+lFl1.get(i)[1]+"/"+lFl1.get(i)[2]+" "
                             +" dist="+Math.sqrt(lFl1.get(i)[0]*lFl1.get(i)[0]+lFl1.get(i)[1]*lFl1.get(i)[1]+lFl1.get(i)[2]*lFl1.get(i)[2]));
                         */
                    }
                    //System.out.println("Found " + lFl1.size() + " values for BACK line.");
                    List<double[]> lFl2 = traceFieldLine(lCube, lInput, lStepSize, 1, lMaxSteps, lStepsReturned);
                    for (int i = 1; i < lFl2.size(); i++) {
                        lFl.getPoints().add(new FieldPoint(lFl2.get(i)[0] * RSun, lFl2.get(i)[1] * RSun, lFl2.get(i)[2] * RSun, lFl2.get(i)[3], lFl2.get(i)[4], lFl2.get(i)[5]));
                        /*System.out.println("Found point "+lFl2.get(i)[0]+"/"+lFl2.get(i)[1]+"/"+lFl2.get(i)[2]+" "
                             +" dist="+Math.sqrt(lFl2.get(i)[0]*lFl2.get(i)[0]+lFl2.get(i)[1]*lFl2.get(i)[1]+lFl2.get(i)[2]*lFl2.get(i)[2]));
                         */
                    }
                    //linesBg.addChild(drawLine(lFl,true));
                    //System.out.println("Found " + lFl2.size() + " values for FORWARD line.");

                    lFieldLines.getFieldLines().add(lFl);

                }
            }
        } catch (Exception e) {
            e.printStackTrace();
        }
        return lFieldLines;
    }

    /**
     * Get interpolated values from a list of position in a cube
     *
     * @return List of lines
     */
    /**
     *
     * @param pR R position value in km
     * @param pT theta position values in radians
     * @param pP phi position values in radians
     * @param pX X values
     * @param pY Y values
     * @param pZ Z values
     * @param pPointsCoordinates input xyz in
     * @return
     */
    public static List<double[]> listInterpolationPoints3DFromSphere(
            float[] pR, float[] pT, float[] pP,
            float[][][] pX, float[][][] pY, float[][][] pZ,
            List<double[]> pPointsCoordinates) {

        float[] lRArrayCube = pR;
        float[] lThetaArrayCube = pT; //from 0 to Math.PI
        float[] lPhiArrayCube = new float[pP.length]; //from 0 to Math.PI to -0
        for (int i = 0; i < lPhiArrayCube.length; i++) {//get values from 0 to 2*PI
            lPhiArrayCube[i] = (float) ((pP[i] < 0) ? pP[i] + 2 * Math.PI : pP[i]);
        }
        //Array.arraycopy(pCube.getpValues(), 0, lPhiArrayCube, 0, lPhiArrayCube.length);

        float lRg0 = lRArrayCube[0];
        float lTg0 = lThetaArrayCube[0];
        float lPg0 = lPhiArrayCube[0];
        float lStepR = Math.abs(lRArrayCube[1] - lRArrayCube[0]); //Step is not constant!!
        float lStepT = lThetaArrayCube[1] - lThetaArrayCube[0];//Step is not constant!!
        float lStepP = lPhiArrayCube[1] - lPhiArrayCube[0];

//        for(float lRVal:lRArrayCube) {
//            System.out.println("Values T:"+lRVal);
//        }
//        for(int i=1;i<lPhiArrayCube.length;i++) {
//            System.out.println("Values P:"+lPhiArrayCube[i]+"/"+(lPhiArrayCube[i]-lPhiArrayCube[i-1]));
//        }
//        System.out.println("---- minMax of each dim: "
//                + "R="+lRg0+"/"+lRArrayCube[lRArrayCube.length-1]+"/"+lStepR
//                +" T="+lTg0+"/"+lThetaArrayCube[lThetaArrayCube.length-1]+"/"+lStepT
//                +" P="+lPg0+"/"+lPhiArrayCube[lPhiArrayCube.length-1]+"/"+lStepP);
        float lrMin = lRArrayCube[0];
        float lrMax = lRArrayCube[lRArrayCube.length - 1];
        //System.out.println("---- KKH lrMin:" + lrMin + " ; lrMax:" + lrMax);

        List<double[]> lOutPointsCube = new ArrayList<double[]>();
        float lValTmp = 0;
        int lCptTmp = 0, lCptInSph = 0, lCptOutSph = 0;
        for (double[] lPt : pPointsCoordinates) {
            lCptTmp++;
            Point3f lPtSpherical = cart2Spheric(new Point3f(
                    (float) lPt[0], (float) lPt[1], (float) lPt[2]));

            double lPtSph_P = (lPtSpherical.getX() < 0) ? lPtSpherical.getX() + 2 * Math.PI : lPtSpherical.getX();
            //double lPtSph_P = lPtSpherical.getX();
            double lPtSph_T = lPtSpherical.getY();
            double lPtSph_R = lPtSpherical.getZ();

            int lRg = (int) ((lPtSph_R - lRg0) / lStepR);//Step is constant!!
            int lTg = (int) ((lPtSph_T - lTg0) / lStepT);//Step is constant!!
            int lPg = (int) ((lPtSph_P - lPg0) / lStepP);

            /*System.out.println(" - KKH RTP idxs: R:" + lPtSph_R+"/"+lRg0+"/"+lRg
                    + " ; T:" + lPtSph_T+"/"+lTg0+"/"+lTg
                    + " ; P:" + lPtSph_P+"/"+lPg0+"/"+lPg);*/
//            System.out.println(" - KKH rg0:" + rg0 + " ; tg0:" + tg0 + " ; pg0:" + pg0);           
//            System.out.println(" - KKH Xg:" + Xg + " ; Yg:" + Yg + " ; Zg:" + Zg);
            double[] lOutPt = new double[6];

            lOutPt[0] = lPt[0];
            lOutPt[1] = lPt[1];
            lOutPt[2] = lPt[2];

            if ((lRg + 1 >= lRArrayCube.length
                    || lTg + 1 >= lThetaArrayCube.length
                    || lRg < 0 || lTg < 0 || lPg < 0)
                    || !(lrMin <= lPtSph_R && lPtSph_R <= lrMax) //outside sphere
                    ) {
                //System.out.println(" - KKH OUT() R:" + lPtSph_R + " ; P:" + lPtSph_P + " ; T:" + lPtSph_T);
                lOutPt[3] = Double.NaN;
                lOutPt[4] = Double.NaN;
                lOutPt[5] = Double.NaN;
                lOutPointsCube.add(lOutPt);
                lCptOutSph++;
                continue;

            }
            //loop on phi
            int lPIdx = lPg % lPhiArrayCube.length;
            int lP1Idx = (lPg + 1) % lPhiArrayCube.length;
            float tx = ((float) lPtSph_R - lRArrayCube[lRg]) / (lRArrayCube[lRg + 1] - lRArrayCube[lRg]);
            float ty = ((float) lPtSph_T - lThetaArrayCube[lTg]) / (lThetaArrayCube[lTg + 1] - lThetaArrayCube[lTg]);
            float tz = ((float) lPtSph_P - lPhiArrayCube[lPIdx]) / (lPhiArrayCube[lP1Idx] - lPhiArrayCube[lPIdx]);
            if (lP1Idx == 0) {
                //if looping on other side, remove 360°
                float tz2 = (float) ((lPtSph_P - lPhiArrayCube[lPIdx]) / (lPhiArrayCube[lP1Idx] + 2 * Math.PI - lPhiArrayCube[lPIdx]));
//                System.out.println("looping on z "+lPIdx+"/"+lP1Idx+"/ "+tz+":"+tz2+" / "
//                        +lPtSph_P+"/"+lPhiArrayCube[lPIdx]+"/"+lPhiArrayCube[lP1Idx]);
                tz = tz2;
            }

            lOutPt[3] = trilinInterpol(pX, lRg, lTg, lPIdx, tx, ty, tz);
            lOutPt[4] = trilinInterpol(pY, lRg, lTg, lPIdx, tx, ty, tz);
            lOutPt[5] = trilinInterpol(pZ, lRg, lTg, lPIdx, tx, ty, tz);
            //System.out.println("Found interpolated pt Pxyz/Bxyz = "+Arrays.toString(lOutPt));
            lOutPointsCube.add(lOutPt);
            lCptInSph++;

        }

        //System.out.println(" - KKH listInterpolationPointsFromSphere() lCptInSphere:" + lCptInSph + " Total:" + lCptTmp);
        return lOutPointsCube;

    }

    /**
     * 3D linear interpolation in a cube
     *
     * @param pValues
     * @param pX voxel X position
     * @param pY voxel Y position
     * @param pZ voxel Z position
     * @param pTx distance from pX
     * @param pTy distance from pY
     * @param pTz distance from pZ
     * @return
     */
    public static float trilinInterpol(float[][][] pValues, int pX, int pY, int pZ, float pTx, float pTy, float pTz) {
        int lZ1 = pZ + 1;
        if (pZ + 1 == pValues[pX][pY].length) {
            lZ1 = 0;
            //System.out.println("trilin: loop on Z "+pX+"/"+pY+"/"+(pZ)+" "+pTx+"/"+pTy+"/"+(pTz));
        }//loop on Phi for spherical
        float c000 = pValues[pX][pY][pZ];
        float c100 = pValues[pX + 1][pY][pZ];
        float c010 = pValues[pX][pY + 1][pZ];
        float c110 = pValues[pX + 1][pY + 1][pZ];
        float c001 = pValues[pX][pY][lZ1];
        float c101 = pValues[pX + 1][pY][lZ1];
        float c011 = pValues[pX][pY + 1][lZ1];
        float c111 = pValues[pX + 1][pY + 1][lZ1];

        return (1 - pTx) * (1 - pTy) * (1 - pTz) * c000
                + pTx * (1 - pTy) * (1 - pTz) * c100
                + (1 - pTx) * pTy * (1 - pTz) * c010
                + pTx * pTy * (1 - pTz) * c110
                + (1 - pTx) * (1 - pTy) * pTz * c001
                + pTx * (1 - pTy) * pTz * c101
                + (1 - pTx) * pTy * pTz * c011
                + pTx * pTy * pTz * c111;
    }

    /**
     * Calculate the normalized step vector at position *x*.
     *
     * @param pIn position
     * @param ds step size
     * @param dir step direction
     * @return Double[] containing steped vector (0,1,2) mag vector(3,4,5)
     * @throws ...
     */
    public static double[] calcStepVector(Cube lCube, double[] pIn, double ds, Integer dir) {
        double[] dxOut = new double[6];
        //TODO dx_out = interp(x); //interpolate a vector at point x

        List<double[]> lP3d = new ArrayList<>();
        lP3d.add(pIn);
        List<double[]> lRes = listInterpolationPoints3DFromSphere(
                lCube.getrValues(), lCube.gettValues(), lCube.getpValues(),
                lCube.getbXValues(), lCube.getbYValues(), lCube.getbZValues(),
                lP3d);

        dxOut[0] = lRes.get(0)[3];
        dxOut[1] = lRes.get(0)[4];
        dxOut[2] = lRes.get(0)[5];
        dxOut[3] = lRes.get(0)[3];
        dxOut[4] = lRes.get(0)[4];
        dxOut[5] = lRes.get(0)[5];

        double vmag = Math.sqrt(Math.pow(dxOut[0], 2)
                + Math.pow(dxOut[1], 2)
                + Math.pow(dxOut[2], 2));
        double stepSize = dir * ds / vmag;
        for (int i = 0; i < 3; i++) {
            dxOut[i] = dxOut[i] * stepSize;
        }
        return dxOut;
    }

    /**
     * Update a position vector *x* by a single RK4 step along the vector grid.
     *
     * @param x position
     * @param ds step size
     * @param dir step direction (1 or -1)
     * @return Double[] containing normalized vector at position x
     * @throws ...
     */
//TODO x = position input/output: laisser x input et ajouter return x updated	
    public static double[] RK4Step(Cube lCube, double[] x, double ds, Integer dir) {
        double[] xout = new double[6];
        //Step 1
        double[] dx1 = calcStepVector(lCube, x, ds, dir);
        //Step 2
        double[] x2 = new double[3];
        for (int i = 0; i < 3; i++) {
            x2[i] = x[i] + 0.5 * dx1[i];
        }
        double[] dx2 = calcStepVector(lCube, x2, ds, dir);
        // Step 3
        double[] x3 = new double[3];
        for (int i = 0; i < 3; i++) {
            x3[i] = x[i] + 0.5 * dx2[i];
        }
        double[] dx3 = calcStepVector(lCube, x3, ds, dir);
        // Step 4
        double[] x4 = new double[3];
        for (int i = 0; i < 3; i++) {
            x4[i] = x[i] + dx3[i];
        }
        double[] dx4 = calcStepVector(lCube, x4, ds, dir);

        for (int i = 0; i < 3; i++) {
            xout[i] = x[i] + (dx1[i] + 2 * dx2[i] + 2 * dx3[i] + dx4[i]) / 6.;
        }
        for (int i = 3; i < 6; i++) {
            xout[i] = (dx1[i] + 2 * dx2[i] + 2 * dx3[i] + dx4[i]) / 6.;
        }
        return xout;
    }

    /**
     * Check if x is within bounds of the grid.
     *
     * @param pIn seed point
     * @param pDir direction to trace (1=forward, -1=backward)
     * @param pStepIn max steps
     * @param pStepOut max steps really integrated (not working)
     * @param pStep step size
     * @return fieldLine
     * @throws ...
     */
    public static List<double[]> traceFieldLine(Cube lCube, double[] pIn, double pStep, Integer pDir, Integer pStepIn, Integer pStepOut) {

        // Initialize all field line points to (0, 0, 0) 
        List<double[]> fieldLine = new ArrayList<>();
        // Set the initial point to be the seed
        //fieldLine.add(new double[]{pIn[0],pIn[1],pIn[2]});
        int ns = pStepIn;
        //pStepOut = 1; // number of taken steps
        double[] xs;
        double[] x = new double[3];
        x[0] = pIn[0];
        x[1] = pIn[1];
        x[2] = pIn[2];

        fieldLine.add(RK4Step(lCube, x, 0, pDir)); //first point
        for (int i = 1; i < ns; i++) {
            // Do a single step
            xs = RK4Step(lCube, x, pStep, pDir);
            // Check if we are out of bounds
            //System.out.println("Found Step ["+xs[0]+","+xs[1]+","+xs[2]+",");
            if (Double.isNaN(xs[0]) || Double.isNaN(xs[1]) || Double.isNaN(xs[2])) {
                //System.out.println("Found NaN, Reducing step.");
                double lStep = pStep / 2;
                int j = 2;
                for (j = 2; j < 10; j++) {//finish line
                    xs = RK4Step(lCube, x, pStep / j, pDir);
                    if (!Double.isNaN(xs[0]) && !Double.isNaN(xs[1]) && !Double.isNaN(xs[2])) {
                        break;
                    }
                }
                if (j == 10) {
                    break;
                }
            }
            // Save the step value
            fieldLine.add(xs);
            x = xs;
//            for (int j = 0; j < 3; j++) {
//                fieldLine[i][j] = xs[j];
//                x[j] = xs[j];
//            }
            pStepOut = i;
        }
        //System.out.println("Found " + fieldLine.size() + " values");
        return fieldLine;
    }

    /**
     * Compute cartesian coordinates from spherical longitude and latitude
     * Specific method for PFSS cube reading. can be replaced once tests done.
     *
     * @param pLong Phi in radians.
     * @param pLat theta in radians.
     * @param pRadius Vector length.
     * @return Cartesian coordinates.
     */
    public static Point3f sphericToCart(double pLong, double pLat, double pRadius) {
        //System.out.println("Convert long: "+Math.toDegrees(pLong));
        Point3f p3d = new Point3f();
        double lColat = Math.PI / 2 - pLat;
        double lLong = pLong;
        p3d.x = (float) (pRadius * Math.cos(lLong) * Math.sin(lColat));
        p3d.y = (float) (pRadius * Math.sin(lLong) * Math.sin(lColat));
        p3d.z = (float) (pRadius * Math.cos(lColat));
        return p3d;
    }

    /**
     * Compute cartesian coordinates from spherical longitude and latitude
     *
     * @param pP3f Point in IAU frame, in km
     * @return Phi, Theta, R.
     */
    public static Point3f cart2Spheric(Point3f pP3f) {
        Point3f lP3f = new Point3f();
        double lBig = Math.max(Math.max(Math.abs(pP3f.x), Math.abs(pP3f.y)), Math.abs(pP3f.z));
        double lRadius = 0;
        double lLongitude = 0;
        double lCoLatitude = 0;
        if (lBig > 0) {

            lRadius = Math.sqrt(pP3f.x * pP3f.x + pP3f.y * pP3f.y + pP3f.z * pP3f.z);
            lCoLatitude = Math.atan2(Math.sqrt(Math.pow(pP3f.x, 2) + Math.pow(pP3f.y, 2)), pP3f.z);
            if (pP3f.x == 0 && pP3f.y == 0) {
                lLongitude = 0;
            } else {
                lLongitude = Math.atan2(pP3f.y, pP3f.x);
            }

        } else {
            lLongitude = 0;
            lCoLatitude = 0;
            lRadius = 0;
        }

        //convert colat to lat and radians to degrees
        lP3f.x = (float) lLongitude;
        lP3f.y = (float) (Math.PI / 2 - lCoLatitude);
        lP3f.z = (float) lRadius;
        return lP3f;
    }
}
