/* davey17-bv.rr
  境界値問題.
*/
import("names.rr");;
import("glib3.rr");;
Glib_math_coordinate=1;;

module davey_bvp;
localf to_float;
localf inner_prod_real;
localf mat_vec_mult;
localf mat_mult;
localf eval_a;
localf rk4_step_n;
localf rk4_adaptive_step_dqr_bvp;
localf solve_bvp_airy;
localf plot_path;

def to_float(Z) { return number_eval(Z); }

def inner_prod_real(V1, V2) {
    S = 0; N = length(V1);
    for (I=0; I<N; I++) S += V1[I]*V2[I];
    return to_float(S);
}

def mat_vec_mult(A, V) {
    N = length(V); Res = newvect(N);
    for (I=0; I<N; I++) {
        S = 0;
        for (J=0; J<N; J++) S += A[I][J]*V[J];
        Res[I] = to_float(S);
    }
    return Res;
}

// 行列同士の積 (Phi の更新用)
def mat_mult(A, B) {
    if (type(A)==6) N=size(A)[0];
    else N = length(A);
    Res = newmat(N, N);
    for (I=0; I<N; I++) {
        for (J=0; J<N; J++) {
            S = 0;
            for (K=0; K<N; K++) S += A[I][K]*B[K][J];
            Res[I][J] = to_float(S);
        }
    }
    return Res;
}

def eval_a(A_mat, X_var, X_val) {
    N = length(A_mat); A_eval = newmat(N, N);
    for (I=0; I<N; I++) {
        for (J=0; J<N; J++) {
            Val = base_replace(A_mat[I][J], [[X_var, X_val]]);
            A_eval[I][J] = to_float(Val);
        }
    }
    return A_eval;
}

def rk4_step_n(A_mat, X_var, X, Y_list, H) {
    N = length(Y_list);
    A1 = eval_a(A_mat, X_var, X);
    K1 = newvect(N); for(I=0;I<N;I++) K1[I] = mat_vec_mult(A1, Y_list[I]);
    Y_t1 = newvect(N); for(I=0;I<N;I++) { V=newvect(N); for(J=0;J<N;J++) V[J]=to_float(Y_list[I][J]+0.5*H*K1[I][J]); Y_t1[I]=V; }
    
    A2 = eval_a(A_mat, X_var, X+0.5*H);
    K2 = newvect(N); for(I=0;I<N;I++) K2[I] = mat_vec_mult(A2, Y_t1[I]);
    Y_t2 = newvect(N); for(I=0;I<N;I++) { V=newvect(N); for(J=0;J<N;J++) V[J]=to_float(Y_list[I][J]+0.5*H*K2[I][J]); Y_t2[I]=V; }
    
    A3 = A2;
    K3 = newvect(N); for(I=0;I<N;I++) K3[I] = mat_vec_mult(A3, Y_t2[I]);
    Y_t3 = newvect(N); for(I=0;I<N;I++) { V=newvect(N); for(J=0;J<N;J++) V[J]=to_float(Y_list[I][J]+H*K3[I][J]); Y_t3[I]=V; }
    
    A4 = eval_a(A_mat, X_var, X+H);
    K4 = newvect(N); for(I=0;I<N;I++) K4[I] = mat_vec_mult(A4, Y_t3[I]);
    
    Y_new = newvect(N);
    for(I=0;I<N;I++) { V=newvect(N); for(J=0;J<N;J++) V[J]=to_float(Y_list[I][J]+(H/6.0)*(K1[I][J]+2.0*K2[I][J]+2.0*K3[I][J]+K4[I][J])); Y_new[I]=V; }
    return Y_new;
}

// BVP 用の適応的ステップ (履歴保存のため R_mat を返す)
def rk4_adaptive_step_dqr_bvp(A_mat, X_var, X, Y_list, H, Tol) {
    N = length(Y_list);
    while (1) {
        Y_f = rk4_step_n(A_mat, X_var, X, Y_list, H);
        H_h = H / 2.0;
        Y_h1 = rk4_step_n(A_mat, X_var, X, Y_list, H_h);
        Y_h2 = rk4_step_n(A_mat, X_var, X + H_h, Y_h1, H_h);
        
        Max_Err = 0;
        for (I=0; I<N; I++) {
            Sum_sq = 0; for(J=0;J<N;J++) Sum_sq += (Y_f[I][J] - Y_h2[I][J])^2;
            Err_Y = to_float(Sum_sq^(1/2));
            if (Err_Y > Max_Err) Max_Err = Err_Y;
        }
        
        if (Max_Err < 1e-15) Max_Err = 1e-15;
        Factor = 0.9 * to_float((Tol / Max_Err)^(1/5));
        if (Factor > 2.0) Factor = 2.0; if (Factor < 0.2) Factor = 0.2;
        
        if (Max_Err <= Tol || H < 1e-8) {
            Y_new = newvect(N); R_mat = newmat(N, N);
            for (I=0; I<N; I++) {
                Y_tmp = newvect(N); for(K=0;K<N;K++) Y_tmp[K] = Y_h2[I][K];
                for (J=0; J<I; J++) {
                    Dot = inner_prod_real(Y_h2[I], Y_new[J]); R_mat[J][I] = Dot; 
                    for(K=0;K<N;K++) Y_tmp[K] = to_float(Y_tmp[K] - Dot * Y_new[J][K]);
                }
                Norm = to_float(inner_prod_real(Y_tmp, Y_tmp)^(1/2)); R_mat[I][I] = Norm;
                V_norm = newvect(N); for(K=0;K<N;K++) V_norm[K] = to_float(Y_tmp[K] / Norm);
                Y_new[I] = V_norm;
            }
            return [to_float(X + H), Y_new, R_mat, H * Factor];
        } else {
            H = H * Factor; 
        }
    }
}

// BVP のメインソルバ
def solve_bvp_airy() {
    printf("--- Airy 境界値問題ソルバ (Discrete QR + Backward Reconstruction) ---\n");
    
    A_mat = [[0, 1], [x, 0]];
    
    X = -10.0; X_end = 5.0;
    H = 0.05; Tol = 1e-7;
    
    // 目標とする境界条件
    Y_left_target  = 0.0402412379; // Ai(-10) の近似値
    Y_right_target = 0.0001083444; // Ai(5) の近似値
    
    // --- Phase 1: 前進積分と履歴保存 ---
    Phi = newmat(2, 2); Phi[0][0] = 1.0; Phi[1][1] = 1.0; // Phi_total
    History = []; // [X, Y_list, R_mat] を記録するリスト
    
    Y_list = newvect(2);
    Y_list[0] = newvect(2, [1.0, 0.0]);
    Y_list[1] = newvect(2, [0.0, 1.0]);
    
    History = cons([X, Y_list, newmat(2,2)], History); // 初期のダミーR
    
    while (X < X_end) {
        if (X + H > X_end) H = X_end - X;
        if (H < 1e-14) break;
        
        Res = rk4_adaptive_step_dqr_bvp(A_mat, x, X, Y_list, H, Tol);
        X = Res[0]; Y_list = Res[1]; R_mat = Res[2]; H = Res[3];
        
        Phi = mat_mult(Phi, R_mat);
        History = cons([X, Y_list, R_mat], History);
    }
    
    // --- Phase 2: 終点での境界条件マッチング ---
    // 左境界条件: y(-10) = (Phi * C_end)[0] = Y_left_target
    Phi=map(number_float_to_rational,Phi);
    Y_left_target=number_float_to_rational(Y_left_target);
    Eq1 = Phi[0][0]*c0 + Phi[0][1]*c1 - Y_left_target;
    // 右境界条件: y(5) = (Y_end * C_end)[0] = Y_right_target
    Y_list=map(number_float_to_rational,Y_list);
    Y_right_target=number_float_to_rational(Y_right_target);
    Eq2 = Y_list[0][0]*c0 + Y_list[1][0]*c1 - Y_right_target;
    
    Ans = poly_solve_linear([Eq1, Eq2], [c0, c1]);
    if (length(Ans) == 0) {
        printf("Error: 境界条件の連立方程式が解けません\n"); return 0;
    }
    C_val = newvect(2, [to_float(Ans[0][1]), to_float(Ans[1][1])]);
    printf("終点 X=5 での係数 C_M: %a\n", C_val);
    
    // --- Phase 3: 後退代入による解の再構成 ---
    Path = [];
    for (L = History; L != []; L = cdr(L)) {
        State = car(L);
        Cur_X = State[0]; Cur_Y_list = State[1]; Cur_R_mat = State[2];
        
        // 真の解 y(x) の計算
        Y_true = Cur_Y_list[0][0]*C_val[0] + Cur_Y_list[1][0]*C_val[1];
        Path = cons([Cur_X, [Y_true]], Path); // 逆順で辿りながら cons するので、最終的に正順になる
        
        // 係数を 1ステップ分「過去」に巻き戻す (C_prev = R * C_cur)
        if (cdr(L) != []) {
            C_prev = newvect(2);
            C_prev[0] = to_float(Cur_R_mat[0][0]*C_val[0] + Cur_R_mat[0][1]*C_val[1]);
            C_prev[1] = to_float(Cur_R_mat[1][0]*C_val[0] + Cur_R_mat[1][1]*C_val[1]);
            C_val = C_prev;
        }
    }
    return Path;
}

def plot_path(Path) {
    if (length(Path) < 2) return 0;
    X_min = Path[0][0]; X_max = Path[length(Path) - 1][0];
    Max_abs_y = 0.0;
    for (I = 0; I < length(Path); I++) {
        Val = Path[I][1][0]; if (Val < 0) Val = -Val;
        if (Val > Max_abs_y) Max_abs_y = Val;
    }
    if (Max_abs_y == 0) Max_abs_y = 1.0;
    
    glib_window(X_min - 0.5, -Max_abs_y * 1.1, X_max + 0.5, Max_abs_y * 1.1);;
    for (I = 0; I < length(Path) - 1; I++) {
        glib_line(Path[I][0], Path[I][1][0], Path[I+1][0], Path[I+1][1][0] | color=0xff0000);;
    }
    return 1;
}

endmodule;;

// テスト実行
def test_bvp() {
    Path = davey_bvp.solve_bvp_airy();
    if (Path != 0) davey_bvp.plot_path(Path);
    return 1;
}

test_bvp();;
end;;