/*
 *  Compute Gradient of I
 */
int Lgm_Grad_I( Lgm_Vector *v0, Lgm_Vector *GradI, Lgm_MagModelInfo *mInfo ) {

    Lgm_Vector  u, Pa, Pb;
    double  rat, H, h, a, b, SS, Sa, Sb, I, f[6], r;
    int     i, N;
    


    /*
     * We want to compute the gradient of I at the point v0.
     * This requires 3 derivatives: one each in x, y and z directions.
     * We will try a fairly acurate difference scehme:
     *
     *      f_0^(1) = 1/(60h)  ( f_3 - 9f_2 + 45f_1  - 45f_-1 + 9f_-2 - f_-3 )
     * See page 450 of CRC standard Math tables 28th edition.
     */


    /*
     *  Set h to a smallish value
     */
//    h = 5e-2;
    h = 0.1;
//    h = 0.2;


    switch ( DIFF_SCHEME ) {
        case USE_SIX_POINT:
            N = 3;
            break;
        case USE_FOUR_POINT:
            N = 2;
            break;
        case USE_TWO_POINT:
            N = 1;
            break;
    }


    // User should set these?
    mInfo->Lgm_I_Integrator        = DQAGS;
    mInfo->Lgm_I_Integrator_epsabs = 0.0;
    mInfo->Lgm_I_Integrator_epsrel = 1e-5;

    
    /* X-component */
    mInfo->UseInterpRoutines = 1;
    if (mInfo->VerbosityLevel > 0) printf("\t\tComputing dIdx: h = %g\n", h);
    for (i=-N; i<=N; ++i){

        if (i!=0) { // dont need the center value in our difference scheme
    
            u = *v0; H = (double)i*h; u.x += H;

            /*
             * Trace to southern mirror point
             */
            if ( Lgm_TraceToMirrorPoint( &u, &Pa, &Sa, mInfo->Bm, -1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                /*
                 * Trace to northern mirror point
                 */
                if ( Lgm_TraceToMirrorPoint( &Pa, &Pb, &SS, mInfo->Bm, 1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                    r  = Lgm_Magnitude( &Pb );
                    //mInfo->Hmax = SS/200.0;
                    mInfo->Hmax = SS/(double)mInfo->nDivs;
                    Lgm_TraceLine2( &Pa, &Pb, (r-1.0)*Re, 0.5*SS-mInfo->Hmax, 1.0, 1e-7, FALSE, mInfo );

                    ReplaceFirstPoint( 0.0, mInfo->Bm, &Pa, mInfo );
                    AddNewPoint( SS,  mInfo->Bm, &Pb, mInfo );
                    InitSpline( mInfo );

                    mInfo->Lgm_I_integrand_S         = 0.0;
                    mInfo->Lgm_I_integrand_FirstCall = TRUE;
                    mInfo->Lgm_n_I_integrand_Calls   = 0;
                    mInfo->Sm_South = 0.0;
                    mInfo->Sm_North = SS;
                    if ( SS <= 1e-5 ) {
                        // if FL length is small, use an approx expression for I
                        rat = mInfo->Bmin/mInfo->Bm;
                        if ((1.0-rat) < 0.0) {
                            I = 0.0;
                        } else {
                            // Eqn 2.66b in Roederer
                            I = SS*sqrt(1.0 - rat);
printf("HEREEEEEEEEEEEEEEEEEEEEEEe\n");
                        }
                    } else {
                        I = Iinv_interped( mInfo  );
                    }

                    if (mInfo->VerbosityLevel > 2) printf("I = %g Lgm_n_I_integrand_Calls = %d\n", I, mInfo->Lgm_n_I_integrand_Calls );

                    FreeSpline( mInfo );



                } else {
                    printf("\t\tMirror point below %g km in Southern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                    exit(0);
                }

            } else {
                printf("\t\tMirror point below %g km in Northern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                exit(0);
            }


            f[i+N] = I;

        }

    }
    if (DIFF_SCHEME == USE_SIX_POINT){
        GradI->x = (f[6] - 9.0*f[5] + 45.0*f[4] - 45.0*f[2] + 9.0*f[1] - f[0])/(60.0*h);
    } else if (DIFF_SCHEME == USE_FOUR_POINT){
        GradI->x = (-f[4] + 8.0*f[3] - 8.0*f[1] + f[0])/(12.0*h);
    } else if (DIFF_SCHEME == USE_TWO_POINT){
        GradI->x = (f[2] - f[0])/(2.0*h);
    }
    
    

    /* Y-component */
    if (mInfo->VerbosityLevel > 0) printf("\t\tComputing dIdy: h = %g\n", h);
    for (i=-N; i<=N; ++i){

        if (i!=0) { // dont need the center value in our difference scheme
    
            u = *v0; H = (double)i*h; u.y += H;

            /*
             * Trace to southern mirror point
             */
            if ( Lgm_TraceToMirrorPoint( &u, &Pa, &Sa, mInfo->Bm, -1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                /*
                 * Trace to northern mirror point
                 */
                if ( Lgm_TraceToMirrorPoint( &Pa, &Pb, &SS, mInfo->Bm, 1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                    r  = Lgm_Magnitude( &Pb );
                    //mInfo->Hmax = SS/200.0;
                    mInfo->Hmax = SS/(double)mInfo->nDivs;
                    Lgm_TraceLine2( &Pa, &Pb, (r-1.0)*Re, 0.5*SS-mInfo->Hmax, 1.0, 1e-7, FALSE, mInfo );

                    ReplaceFirstPoint( 0.0, mInfo->Bm, &Pa, mInfo );
                    AddNewPoint( SS,  mInfo->Bm, &Pb, mInfo );
                    InitSpline( mInfo );

                    mInfo->Lgm_I_integrand_S         = 0.0;
                    mInfo->Lgm_I_integrand_FirstCall = TRUE;
                    mInfo->Lgm_n_I_integrand_Calls   = 0;
                    mInfo->Sm_South = 0.0;
                    mInfo->Sm_North = SS;
                    if ( SS <= 1e-5 ) {
                        // if FL length is small, use an approx expression for I
                        rat = mInfo->Bmin/mInfo->Bm;
                        if ((1.0-rat) < 0.0) {
                            I = 0.0;
                        } else {
                            // Eqn 2.66b in Roederer
                            I = SS*sqrt(1.0 - rat);
                        }
                    } else {
                        I = Iinv_interped( mInfo  );
                    }

                    if (mInfo->VerbosityLevel > 2) printf("I = %g Lgm_n_I_integrand_Calls = %d\n", I, mInfo->Lgm_n_I_integrand_Calls );
                    FreeSpline( mInfo );

                } else {
                    printf("\t\tMirror point below %g km in Southern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                    exit(0);
                }

            } else {
                printf("\t\tMirror point below %g km in Northern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                exit(0);
            }


            f[i+N] = I;

        }

    }
    if (DIFF_SCHEME == USE_SIX_POINT){
        GradI->y = (f[6] - 9.0*f[5] + 45.0*f[4] - 45.0*f[2] + 9.0*f[1] - f[0])/(60.0*h);
    } else if (DIFF_SCHEME == USE_FOUR_POINT){
        GradI->y = (-f[4] + 8.0*f[3] - 8.0*f[1] + f[0])/(12.0*h);
    } else if (DIFF_SCHEME == USE_TWO_POINT){
        GradI->y = (f[2] - f[0])/(2.0*h);
    }


    /* Z-component */
    if (mInfo->VerbosityLevel > 0) printf("\t\tComputing dIdz: h = %g\n", h);
    for (i=-N; i<=N; ++i){

        if (i!=0) { // dont need the center value in our difference scheme
    
            u = *v0; H = (double)i*h; u.z += H;

            /*
             * Trace to southern mirror point
             */
            if ( Lgm_TraceToMirrorPoint( &u, &Pa, &Sa, mInfo->Bm, -1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                /*
                 * Trace to northern mirror point
                 */
                if ( Lgm_TraceToMirrorPoint( &Pa, &Pb, &SS, mInfo->Bm, 1.0, mInfo->Lgm_TraceToMirrorPoint_Tol, mInfo ) > 0 ) {

                    r  = Lgm_Magnitude( &Pb );
                    //mInfo->Hmax = SS/200.0;
                    mInfo->Hmax = SS/(double)mInfo->nDivs;
                    Lgm_TraceLine2( &Pa, &Pb, (r-1.0)*Re, 0.5*SS-mInfo->Hmax, 1.0, 1e-7, FALSE, mInfo );

                    ReplaceFirstPoint( 0.0, mInfo->Bm, &Pa, mInfo );
                    AddNewPoint( SS,  mInfo->Bm, &Pb, mInfo );
                    InitSpline( mInfo );

                    mInfo->Lgm_I_integrand_S         = 0.0;
                    mInfo->Lgm_I_integrand_FirstCall = TRUE;
                    mInfo->Lgm_n_I_integrand_Calls   = 0;
                    mInfo->Sm_South = 0.0;
                    mInfo->Sm_North = SS;
                    if ( SS <= 1e-5 ) {
                        // if FL length is small, use an approx expression for I
                        rat = mInfo->Bmin/mInfo->Bm;
                        if ((1.0-rat) < 0.0) {
                            I = 0.0;
                        } else {
                            // Eqn 2.66b in Roederer
                            I = SS*sqrt(1.0 - rat);
                        }
                    } else {
                        I = Iinv_interped( mInfo  );
                    }

                    if (mInfo->VerbosityLevel > 2) printf("I = %g Lgm_n_I_integrand_Calls = %d\n", I, mInfo->Lgm_n_I_integrand_Calls );
                    FreeSpline( mInfo );

                } else {
                    printf("\t\tMirror point below %g km in Southern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                    exit(0);
                }

            } else {
                printf("\t\tMirror point below %g km in Northern Hemisphere\n", mInfo->Lgm_LossConeHeight);
                exit(0);
            }


            f[i+N] = I;

        }

    }
    if (DIFF_SCHEME == USE_SIX_POINT){
        GradI->z = (f[6] - 9.0*f[5] + 45.0*f[4] - 45.0*f[2] + 9.0*f[1] - f[0])/(60.0*h);
    } else if (DIFF_SCHEME == USE_FOUR_POINT){
        GradI->z = (-f[4] + 8.0*f[3] - 8.0*f[1] + f[0])/(12.0*h);
    } else if (DIFF_SCHEME == USE_TWO_POINT){
        GradI->z = (f[2] - f[0])/(2.0*h);
    }


    return(0);

}
void InitSplineModule()
{
    InitSpline();
}
/*
 * This version traces FLs from the Earth at the given MLT/mlat instead of trying to find the Bm radially out first.
 */
double ComputeI_FromMltMlat2( double Bm, double MLT, double mlat, double *r, double I0, Lgm_LstarInfo *LstarInfo ) {

    int         reset=1, reset2, TraceFlag;

    double      Bmin, I, Phi, cl, sl, rat, SS1, SS2, SS, Sn, Ss, Htry, Hdid, Hnext, Bs, Be, s, sgn;
    Lgm_Vector  w, u, Pmirror1, Pmirror2, v1, v2, v3, Bvec, P, Ps, u_scale, Bvectmp, Ptmp;
    double      stmp, Btmp;



    /*
     *  First do a trace to identify the FL type and some of its critical points.
     */
    *r = 1.0 + 100.0/Re;
    Phi = 15.0*(MLT-12.0)*RadPerDeg;
    cl = cos( mlat * RadPerDeg ); sl = sin( mlat * RadPerDeg );
    w.x = (*r)*cl*cos(Phi); w.y = (*r)*cl*sin(Phi); w.z = (*r)*sl;
    Lgm_Convert_Coords( &w, &u, SM_TO_GSM, LstarInfo->mInfo->c );
printf("w = %g %g %g\n", w.x, w.y, w.z);
    TraceFlag = Lgm_Trace( &u, &v1, &v2, &v3, LstarInfo->mInfo->Lgm_LossConeHeight, 1e-6, 1e-8, LstarInfo->mInfo );
    LstarInfo->mInfo->Bfield( &v3, &Bvec, LstarInfo->mInfo );
    Bmin = Lgm_Magnitude( &Bvec );
printf("Got here\n");

    if ( TraceFlag != LGM_CLOSED ) {

        if (LstarInfo->VerbosityLevel > 1){ printf( "\t\t\t> Field Line not closed\n" ); }
        return( 9e99 );

    } else if ( Bmin <= Bm ) {

        /*
         * From the minimum B point, attempt to trace along the field to get the northern mirror point.
         */
        P = v3;
        SS1 = 0.0;
        if ( Lgm_TraceToMirrorPoint( &P, &Pmirror1, &SS1, LstarInfo->mInfo->Bm,  1.0, LstarInfo->mInfo->Lgm_TraceToMirrorPoint_Tol, LstarInfo->mInfo ) > 0 )  {

            SS2 = 0.0;
            if ( Lgm_TraceToMirrorPoint( &P, &Pmirror2, &SS2, LstarInfo->mInfo->Bm,  -1.0, LstarInfo->mInfo->Lgm_TraceToMirrorPoint_Tol, LstarInfo->mInfo ) > 0 )  {

            } else {
                if (LstarInfo->VerbosityLevel > 3){ printf( "\t\t\t> Unable to find southern mirror point\n" ); }
                return( 9e99 );
            }

            // total distance between mirror point.
            SS = SS1 + SS2;
            
            // If its really small, just return 0.0 for I
            if ( fabs(SS) < 1e-7) {
                if (LstarInfo->VerbosityLevel > 3){ printf( "\t\t\t> Distance between mirror points is < 1e-7Re, Assuming I=0\n" ); }
                return( 0.0 );
            }

        } else {
            if (LstarInfo->VerbosityLevel > 3){ printf( "\t\t\t> Unable to find northern mirror point\n" ); }
            return( 9e99 );
        }

        /*
         * OK, we have both mirror points. LEts compute I
         */
        I = 9e99;
        LstarInfo->mInfo->Hmax = 0.1;
        LstarInfo->mInfo->Hmax = SS/(double)LstarInfo->mInfo->nDivs;

        /*
         *  This little section is attempting to solve an annoying
         *  precision issue.  If the distance over which we are trying
         *  to trace is too small, too many sub-divisions (i.e. total
         *  steps) will lead to a step size that is too small.  We can
         *  end up with the gridded FL points computed in
         *  Lgm_TraceLine3() such that the distance, s of the final
         *  point is very slightly less than we expect.  This tries to
         *  reduce the number of divisions to avoid this in cases where
         *  the total distance to trace is very small.
         */
        int nDivs;
        if ( SS/LstarInfo->mInfo->nDivs < 1e-6 ) {
            nDivs = SS/1e-6;
            if (nDivs < 10) nDivs = 10;
        } else {
            nDivs = LstarInfo->mInfo->nDivs;
        }

         
        if ( Lgm_TraceLine3( &(LstarInfo->mInfo->Pm_South), SS, nDivs, 1.0, 1e-7, FALSE, LstarInfo->mInfo ) < 0 ) return( 9e99 );

        /*
         *  Set the limits of integration.
         */
        LstarInfo->mInfo->Sm_South = 0.0;
        LstarInfo->mInfo->Sm_North = SS;

        if ( InitSpline( LstarInfo->mInfo ) ) {

            /*
             *  Do I integral with interped integrand.
             */
             I = Iinv_interped( LstarInfo->mInfo );
             if (LstarInfo->VerbosityLevel > 1) {
                printf("\t\t%s  mlat: %13.6g   I: %13.6g   I0: %13.6g   I-I0: %13.6g    [Sa,Sb]: %.8g  %.8g  (nCalls = %d)%s\n",  LstarInfo->PreStr, mlat, I, I0, I-I0, LstarInfo->mInfo->Sm_South, LstarInfo->mInfo->Sm_North, LstarInfo->mInfo->Lgm_n_I_integrand_Calls, LstarInfo->PostStr );
             }
             FreeSpline( LstarInfo->mInfo );

        } else {
            I = 9e99;
        }


    } else {

        if (LstarInfo->VerbosityLevel > 1){ printf( "\t\t\t> Field line min-B is greater than Bm. Bm = %g Bmin = %g\n", Bm, Bmin ); }
        return( 9e99 );

    }


    return( I );
    

}