SkiTurnAnalyzer STA24 ver0.1 python RTK MovingBase BNO085 data　to the Trajectory Graph

STA24_rev0.1.py

# This is a sample Python script.
# 機能：STA24 SystemのログデータをCSV形式で読み込んで、データの検査と修復をおこなって各デバイス間の同期を整頓する
# 　iTOW抜け：ループで回して、発見したら補間した行を作って、DFの再下端に追加していく、全部ループが終了したらiTOWをSORTして整頓
# graph00では、1個のグラフだったので、01では、全体を網羅するグラフ散布図ベクトル矢印を作る　
# BootsOnのデータも読み込む
#py_CSVread_PandasDF04_TurnRL07
import math
import statistics
import numpy
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.cm as cm
import tkinter
import tkinter.filedialog as FileDialog
import os
import sys
import numpy as np
import pickle
import keyboard

from scipy.signal import find_peaks # type: ignore

from sklearn.linear_model import LinearRegression
import statsmodels.api as sm

def is_num(x):  # 数値か文字かを判別する関数
    try:
        float(x)
    except ValueError:
        return False
    else:
        return True


def additions(df):
    # ---headMot Heading 角度補正
    df["headMot360"] = df.headMot_B.multiply(0.00001)
    df["headMot360"] = np.where(df["headMot360"] > 360, df["headMot360"] - 360, df["headMot360"])

    print(df.headMot360)
    # ---Heading角　補正
    df["Heading180"] = df.relPosHeading_R.multiply(0.00001) + 180
    df["Heading360"] = np.where(df["Heading180"] > 360, df["Heading180"] - 360, df["Heading180"])
    print(df.Heading360)

    # ---headMot移動平均３個MA3
    df["headMotMA3"] = df["headMot360"].rolling(3).mean()
    print(df.headMot360, df.headMotMA3)

    # ---差分　diff()
    df["headMotMA3diff"] = df["headMotMA3"].diff()
    # ---差分ゼロ点を１してマーク
    zerocnt = 0
    zeroflag = 0
    zeroflag_1 = 0
    for i in range(2, len(df.index)):
        if df.loc[i, "headMotMA3diff"] * df.loc[i - 1, "headMotMA3diff"] < 0 and df.loc[
            i - 1, "diffzero"] == 0 and i - zeroflag > 4:
            zeroflag = i
            df.loc[i, "diffzero"] = zerocnt
            zerocnt += 1
        else:
            df.loc[i, "diffzero"] = 0
    return df


def A2H(x0, y0, x1, y1):
    dx = x1 - x0
    dy = y1 - y0
    a2 = math.degrees(math.atan2(dy, dx))
    if dx < 0 and dy > 0:
        an = 180 + a2
    else:
        an = 90 - a2
    return an


def additions2(df):
    df["H-Ysa"] = df["Heading360"] - df["yaw"]
    df["H-Ysa360"] = np.where(df["H-Ysa"] > 360, df["H-Ysa"] - 360, df["H-Ysa"])
    df["skid"] = df["headMotMA3"] - df["Heading360"]
    # Heading from Position
    for i in range(1, len(df) - 1):
        x0 = df.loc[i - 1, "relPosE_B"]
        y0 = df.loc[i - 1, "relPosN_B"]
        x1 = df.loc[i, "relPosE_B"]
        y1 = df.loc[i, "relPosN_B"]
        an = A2H(x0, y0, x1, y1)
        df.loc[i, "Hfp"] = an
        # print("Hfp[",i,"]=",an)
    return df


# ||||||||||||||||||||||||||||def readMBFile||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
def readMBFile(filename, basename, head0):
    # print(result.gSpeed_B)
    df = pd.read_csv(filename, names=head0, low_memory=True)  # CSV　Read dfに読み込まれる head0の列名がつく
    print("PATH:", filename, "READ:", basename)  # basenameは、パスをのぞいたファイル名　ファイル名ubx とbnoで処理分ける
    pd.set_option('display.max_columns', 120)  # 120列まで表示
    df_Base = df[df.iloc[:, 0] == 'B']  # id列[:,0]が'B'ならばdf_Baseとして抽出
    df_Base = df_Base.fillna(0)  # NANをゼロに変換
    df_Base = df_Base.reset_index()  # index番号リセット
    df_Base = df_Base.add_suffix('_B', axis=1)  # head0の名称末尾に'_B'をつけてBaseのデータ列命名
    df_Base = df_Base.drop(columns='index_B')  # index_Bは不要なので消去
    df_Base = df_Base.drop(columns='id_B')  # id_Bも不要なので消去　数値以外のデータ除去
    headB = df_Base.columns  # 完成したヘッダ文字LISTを保持
    print("headB=", headB)
    k = []  # itow エラー　list k[]宣言
    # ------------df_Baseをlistへ読み出し-----------
    list_data = []  # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
    list_data = df_Base.to_numpy().tolist()  # list_dataに値を代入
    # print("list_data=",list_data)
    # print("list_data size=", [len(v) for v in list_data])  #
    # -----------------------------------------
    lbase = len(df_Base)  # 全行数
    # lastrow = len(df_Base)
    for i in range(1, lbase):  # 行数ループ　整数iでカウント
        # print("range i=",i)
        if i > 1:  # i-1対策
            itow0 = (int)(df_Base.iloc[i, 0])  # 整数で番地指定しているのでiloc使用
            itow_1 = (int)(df_Base.iloc[i - 1, 0])  # 1個手前のitow_1
            # print(itow0-itow_1)
            c = []  # 補間計算データ行リスト
            k = 0
            if itow0 - itow_1 > 120:  # itow差が120以上あったらエラー
                itowadd = itow_1 + 120
                # print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
                for j in range(0, 55):  # 列をループして平均値計算
                    ss = df_Base.iloc[i, j]
                    # if  is_num(ss):
                    a = float(df_Base.iloc[i, j])
                    b = float(df_Base.iloc[i - 1, j])
                    # print(j,a,b)
                    c.append(int((a + b) / 2))  # 補間行リストに平均値代入
                # add c to list_data
                list_data.append(c)  # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
                # print("c len=",len(c))
                # print("Err made c=",c)
                # print("list_data size=", [len(v) for v in list_data])

    # ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
    df_Base2 = pd.DataFrame(list_data, columns=headB)  # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成

    # 数値以外のデータが含まれている行の抽出
    non_numeric_rows = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
    # print("nonnumericRow=",non_numeric_rows)
    # 数値以外のデータが含まれている列の抽出
    non_numeric_cols = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
    # print("nonnumericCol=",non_numeric_cols)
    # df_Rover2 = df_Base2.fillna(0)
    # df_Base2['iTOWB']=df_Base2['iTOW_B'].astype('int')
    # -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
    df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype('str')
    df_Base2['iTOW_B'] = df_Base2['iTOW_B'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
    df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype("float64")
    # ----------------------------------------------------------------------------------------------------
    print(df_Base2)
    df_Base2.info()
    # pd.set_option('display.max_rows', None)
    df_Base2 = df_Base2.sort_values("iTOW_B", ascending=True)  # SORT iTOWBでSORT
    print("Sorted df_Base2=", df_Base2)
    # ----------CHECK SORT--------------
    itowerr = 0
    for i in range(1, len(df_Base2) - 1):
        itow0 = df_Base2.iloc[i + 1, 0]
        itow_1 = df_Base2.iloc[i, 0]
        if ((itow0 - itow_1) != 120):
            itowerr += 1
            # print("BASE***********i=", i, itow0, "itowErr=", itowerr)
    # ============================BASE ITOW 補修終了===============================
    # ========================ROVER iTOW処理======================================
    # df_Base2.info()
    # df_Rover = df[df.iloc[:, 0] == 'R']
    # df_Rover.reset_index(drop=True, inplace=True)
    # df_Rover = df_Rover.add_suffix('_R', axis=1)
    df_Rover = df[df.iloc[:, 0] == 'R']  # id列[:,0]が'B'ならばdf_Baseとして抽出
    df_Rover = df_Rover.fillna(0)  # NANをゼロに変換
    df_Rover = df_Rover.reset_index()  # index番号リセット
    df_Rover = df_Rover.add_suffix('_R', axis=1)  # head0の名称末尾に'_B'をつけてBaseのデータ列命名
    df_Rover = df_Rover.drop(columns='index_R')  # index_Bは不要なので消去
    df_Rover = df_Rover.drop(columns='id_R')  # id_Bも不要なので消去　数値以外のデータ除去
    headR = df_Rover.columns  # 完成したヘッダ文字LISTを保持
    print("headR=", headR)
    # print(df_Rover)
    print("============================df_Rover describe===============================")
    print(df_Rover.describe())
    kr = []  # itow エラー　list kr[]宣言
    # ------------df_Roverをlistへ読み出し-----------
    list_datar = []  # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
    list_datar = df_Rover.to_numpy().tolist()  # list_datarに値を代入
    # print("list_datar=",list_datar)
    print("list_datar size=", [len(v) for v in list_datar])  #
    # ------Rover itow 抜け------------------------------
    lbaser = len(df_Rover)  # 全行
    for i in range(1, lbaser):  # 行数ループ　整数iでカウント
        # print("range i=",i)
        if i > 1:  # i-1対策
            itow0 = (int)(df_Rover.iloc[i, 0])  # 整数で番地指定しているのでiloc使用
            itow_1 = (int)(df_Rover.iloc[i - 1, 0])  # 1個手前のitow_1
            # print(itow0-itow_1)
            cr = []  # 補間計算データ行リスト
            kr = 0
            if itow0 - itow_1 == 240:  # itow差が120以上あったらエラー
                itowadd = itow_1 + 120
                print("df_Rover:itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
                for j in range(0, 55):  # 列をループして平均値計算
                    ss = df_Rover.iloc[i, j]
                    # if  is_num(ss):
                    a = float(df_Rover.iloc[i, j])
                    b = float(df_Rover.iloc[i - 1, j])
                    c = int((a + b) / 2)
                    print("Rover Error Row itow0", itow0, ":i,j,a,b=", i, j, a, b)
                    cr.append(c)  # 補間行リストに平均値代入
                # add c to list_data
                list_datar.append(cr)  # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
                print("cr len=", len(cr))
            elif itow0 - itow_1 > 240:
                print("Err made cr=", cr)
            #else:
                #print("itow Check OK i=", i, itow0, itow_1)
                # print("list_datar size=", [len(v) for v in list_datar])

    # ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
    df_Rover2 = pd.DataFrame(list_datar, columns=headR)  # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成
    # df_Rover2.to_csv(r'C:/RTK_Log/df_Rover2_' + basename)
    # 数値以外のデータが含まれている行の抽出
    non_numeric_rows = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
    # print("nonnumericRow=", non_numeric_rows)
    # 数値以外のデータが含まれている列の抽出
    non_numeric_cols = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
    # print("nonnumericCol=", non_numeric_cols)
    df_Rover2 = df_Rover2.fillna(0)
    # df_Rover2['iTOW_R']=df_Rover2['iTOW_R'].astype('int')
    # -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
    df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype('str')
    df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
    df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype("float64")
    # ----------------------------------------------------------------------------------------------------

    # print(df_Rover2)
    df_Rover2.info()
    # pd.set_option('display.max_rows', None)
    df_Rover2 = df_Rover2.sort_values("iTOW_R", ascending=True)  # SORT iTOW_RでSORT
    # print("Sorted df_Rover2=", df_Rover2)
    # ----------CHECK SORT--------------
    itowerr = 0
    for i in range(1, len(df_Rover2) - 1):
        itow0 = df_Rover2.iloc[i + 1, 0]
        itow_1 = df_Rover2.iloc[i, 0]
        if ((itow0 - itow_1) != 120):
            itowerr += 1
            print("***********i=", i, itow0, "itowErr=", itowerr, "sa=", itow0 - itow_1)
    # ============================Rover ITOW 補修終了===============================
    # df_Base2とdf_Rover2の長さを短いほうにあわせる
    # drop(range(削除したい最初の行, 削除したい最後の行+1), axis=”rows“)
    # print("df_Base2 tail=", df_Base2.tail())
    # print("df_Rover2 tail=", df_Rover2.tail())
    Base2len = len(df_Base2)
    Rover2len = len(df_Rover2)
    if Base2len > Rover2len:
        last_row = df_Base2.shape[0] - 1
        upper_row = last_row - (len(df_Base2) - len(df_Rover2))
        df_Base2 = df_Base2.drop(range(upper_row, last_row), axis="rows")
    elif len(df_Base2) < len(df_Rover2):
        last_row = df_Rover2.shape[0] - 1
        upper_row = last_row - (len(df_Rover2) - len(df_Base2))
        df_Rover2 = df_Rover2.drop(range(upper_row, last_row), axis="rows")
    df_Base2.reset_index()
    df_Rover2.reset_index()
    # print("df_Base2 len=", len(df_Base2), "df_Rover2 len=", len(df_Rover2))
    print("===================df_Rover2 describe===========================")
    print(df_Rover2.describe())
    print("--------------------CONCATでfd_Base2とfd_Rover2を横に合体BR_ファイルCSV保存---------------------")
    result = pd.concat([df_Base2, df_Rover2], axis=1)
    result = result.fillna(0)  # NANをゼロに変換
    result = result.reset_index(drop=True)

    result = result.drop(result.index[-1])
    # print("relposHeading Max=", result["relPosHeading_B"].max(), "min=", result["relPosHeading_B"].min())
    additions(result)
    result.to_csv(r'C:/RTK_Log/BR_' + basename)#BR_ファイル保存で完成
    # print(result)
    print("============================result describe=========================================")
    print(result.describe())
    return result


def plotheadmot(result, basename):
    # ===========================================================================================
    # ===================PLOT headMot Heading diff NutralPoint===================================
    # =========================================================================================
    # １つのfigにたくさんグラフを作る
    # fig, ax = plt.subplots(2, 2, figsize=(12, 10))
    fig, ax = plt.subplots(figsize=(12, 10))
    fig.suptitle(basename)

    # graph1
    # ax[0,0].plot(result.index,result.gSpeed_B,"green","gSpeed_B")# Graph1 gSpeed
    # graph2
    # ax2= ax.twinx()
    # ax[0, 0].plot(result.index, result.headMot360, "red", "headMot360")#Graph2  headMot
    # ax[0, 0].plot(result.index, result.Heading360, "blue", "Heading360")#Graph2 Heading
    ax.plot(result.index, result.headMot360, "red", label="headMot360")  # Graph2  headMot
    ax.plot(result.index, result.Heading360, "blue", label="Heading360")  # Graph2 Heading
    # ax[0,1].plot(result.index,result.headMotMA3,"black","headMotMA3")#Graph3  headMotMA3
    # ax2 = ax[0,0].twinx()#2軸作成
    ax2 = ax.twinx()  # 2軸作成
    ax2.plot(result.index, result.headMotMA3diff, "black", label="headMotMA3diff")  # Graph3  headMotMA3diff
    ax2.grid()
    # 縦線

    for i in range(2, len(result.index)):
        if result.loc[i, "headMotMA3diff"] * result.loc[i - 1, "headMotMA3diff"] < 0:
            ax2.axvline(i, color="aqua")
            result.diffzero = 1
        else:
            result.diffzero = 0
    # graph3
    result['relPosN_B'] = result['relPosN_B'].astype('int')
    result['relPosE_B'] = result['relPosE_B'].astype('int')
    # print(result.relPosE_B)
    # print(result.relPosN_B)
    # ax[1, 0].scatter(result.relPosE_B, result.relPosN_B)#Graph4  BaseXY

    ax.legend(loc="upper left")
    ax2.legend(loc="center left")
    # ax2.legend()
    plt.show()


# ----------------PLOT VECTOR-----------------------------------------------------
def resultrange(result):
    # headmotMA3のMaxMin
    hmax = result["headMotMA3"].max()
    hmin = result["headMotMA3"].min()

    return hmax, hmin


def arrowmake(df, index):
    x0 = df.loc[index, "relPosE_B"]
    y0 = df.loc[index, "relPosN_B"]
    x1 = df.loc[index + 1, "relPosE_B"]
    y1 = df.loc[index + 1, "relPosN_B"]
    dist = numpy.sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2) * 0.8
    # headMot
    hmotdeg = df.loc[index, "headMot360"]
    hmotrad = math.radians(df.loc[index, "headMot360"])
    vxm = dist * math.sin(hmotrad)
    vym = dist * math.cos(hmotrad)
    # print("vxm=", vxm, "vym=", vym)
    # Heading
    headdeg = df.loc[index, "Heading360"]
    headrad = math.radians(df.loc[index, "Heading360"])
    hxm = dist * math.sin(headrad)
    hym = dist * math.cos(headrad)
    gsp = df.loc[index, "gSpeed_B"] * 0.001 * 3600 * 0.001
    skid = hmotdeg - headdeg
    high = df.loc[index, "height_B"] * 0.001
    edge = df.loc[index, "roll"]

    return vxm, vym, hxm, hym, gsp, skid, high, edge


# Nutral Point検索　diffとRollとheadMot から
def getnutral(df):
    npt = []  # Nutral Point index No
    npx = []  # Nutral Point relPosE
    npy = []  # Nutral Point relPosN
    npt_1 = 1
    nptc = 0
    for i in range(1, len(df.index) - 1):
        ar = df.loc[i, "roll"]
        ar_1 = df.loc[i - 1, "roll"]
        if ar * ar_1 < 0 and i - npt_1 > 3:
            npt_1 = i
            npt.append(i)
            npx.append(df.loc[i, "relPosE_B"])
            npy.append(df.loc[i, "relPosN_B"])
            nptc += 1
            # ("Nutral Point No=",nptc,"index=",i,"npx=",df.loc[i,"relPosE_B"],"npy=",df.loc[i, "relPosN_B"])

    return npt, npx, npy


#******************************df_BTplot用追加処理*******************************************************
#NutralPointは、BRBNに合わせる　グラフ原点もBRBNに合わせる
def additions4(df):#df_BTplotの追加列
    df["headMotT360"] = df.headMot_T.multiply(0.00001)
    df["headMotT360"] = np.where(df["headMotT360"] > 360, df["headMotT360"] - 360, df["headMotT360"])
    df["headMot_TMA3"] = df["headMotT360"].rolling(3).mean()
    # ---headMot_T差分　diff()
    df["headMotTMA3diff"] = df["headMot_TMA3"].diff()
    # -----------headMot_TMA3とyaw_Tのpeaks 検出して補正値計算-------------------------------
    peaks, _ = find_peaks(df["headMot_TMA3"], distance=8)
    ypeaks, _ = find_peaks(df["yaw_T"], distance=12)
    print(peaks)
    print(ypeaks)
    df["headMot_peaks"]=0
    df["yawT_peaks"]=0
    df["yawT_peakPos"]=0
    df["yawHosei"]=0
    df["headMot_peaks"]=df["headMot_peaks"].astype("float64")
    df["yawT_peaks"]=df["yawT_peaks"].astype("float64")
    df["yawT_peakPos"]=df["yawT_peakPos"].astype("float64")
    df["yawHosei"]=df["yawHosei"].astype("float64")
    headMotmax=[]
    headMotindex=[]
    yawmax=[]
    yawindex=[]
    #df = df.astype('float64')
    #peaksに近いypeaksを探す
    if len(peaks)>len(ypeaks):
        ln=len(ypeaks)
    else:
        ln=len(peaks)
    for i in range(0,ln-1):
            print("i=",i)
            headMotindex.append(peaks[i])
            crow=peaks[i]
            headMotmax.append(df.loc[crow,"headMot_TMA3"])  
            df.loc[crow,"headMot_peaks"]=headMotmax[i]      
            for j in range(0,len(ypeaks)-1):
                    if peaks[i]>ypeaks[j] and peaks[i]<ypeaks[j+1] :                        
                            maesa=peaks[i]-ypeaks[j+1]
                            atosa=ypeaks[j+1]-peaks[i]
                            if maesa<=atosa:
                                df.loc[ypeaks[j],"yawT_peakPos"]=100#yawpeak位置マーク
                                yawmax.append(df.loc[ypeaks[j],"yaw_T"])
                                yawindex.append(ypeaks[j])
                                df.loc[crow,"yawT_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる                   
                            else:
                                df.loc[ypeaks[j+1],"yawT_peakPos"]=100#yawpeak位置マーク
                                yawmax.append(df.loc[ypeaks[j+1],"yaw_T"])
                                yawindex.append(ypeaks[j+1])
                                df.loc[crow,"yawT_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
                    elif peaks[i]==ypeaks[j] or peaks[i]==ypeaks[j+1]:#headMot peakとyaw peakが等しい時
                            df.loc[ypeaks[j],"yawT_peakPos"]=100  
                            df.loc[crow,"yawT_peaks"]=df.loc[ypeaks[j],"yaw_T"]
                            yawmax.append(df.loc[ypeaks[j],"yaw_T"])
                            yawindex.append(ypeaks[j])

    print("headMotmax:",headMotmax)  
    print("headMotindex:",headMotindex)
    print("yawmax:",yawmax)  
    print("yawindex:",yawindex)
    #補正値で埋める
    df["yawHosei"]=0
    for i in range(0,len(headMotindex)-1):
        for j in range(yawindex[i],yawindex[i+1]):
            df.loc[j,"yawHosei"]=headMotmax[i]-yawmax[i]
            df.loc[j,"yawH"]= df.loc[j,"yawHosei"]+df.loc[j,"yaw_T"]
     #----------yaw角からBase　Rover座標を計算--------------------------------
    lenBR:float64=100#仮想アンテナ間距離100cm
    lenCB:float64=50#BootsアンテナからBaseアンテナまでの距離
    lenCR:float64=lenBR-lenCB#BootsアンテナからRoverアンテナまでの距離
    df["rpNR_BT"]=0#Rover(後アンテナ)N座標 
    df["rpER_BT"]=0#Rover(後アンテナ)E座標
    df["rpNB_BT"]=0#Base(前アンテナ)N座標
    df["rpEB_BT"]=0#Base(前アンテナ)E座標
    df["rpNR_BT"]=df["rpNR_BT"].astype("float64")
    df["rpER_BT"]=df["rpER_BT"].astype("float64")
    df["rpNB_BT"]=df["rpNB_BT"].astype("float64")
    df["rpEB_BT"]=df["rpEB_BT"].astype("float64")
    #df=df.astype("float64")
    
    for i in range(0,len(df)):
        xbt0=df.loc[i,"relPosE_T"]
        ybt0=df.loc[i,"relPosN_T"]
        yawt=df.loc[i,"yawH"]
        yawtrad=math.radians(yawt)
        yawtrad180=math.radians(yawt+180)
        df.loc[i,"rpEB_BT"]=xbt0+lenCB*math.sin(yawtrad)
        df.loc[i,"rpNB_BT"]=ybt0+lenCB*math.cos(yawtrad)
        df.loc[i,"rpER_BT"]=xbt0+lenCR*math.sin(yawtrad180)
        df.loc[i,"rpNR_BT"]=ybt0+lenCR*math.cos(yawtrad180)
        #df.loc[i,"rpNR_BT"]=yr
        #df.loc[i,"rpER_BT"]=xr
        #df.loc[i,"rpNB_BT"]=ybr
        #df.loc[i,"rpEB_BT"]=xb

    df_CPLT = pd.DataFrame()
    df_CPLT = df[["headMot_TMA3", "yawH","rpEB_BT","rpNB_BT","rpER_BT","rpNR_BT","relPosE_T","relPosN_T"]]
    print("csv add4")
    df_CPLT.to_csv(r'C:/RTK_Log/add4_BTplot_' + basename, index=False)  # CSV save
    print(df)
    



    return df
def additions5(df):#resultBRBN用のyaw補正処理
    #df["headMotT360"] = df.headMot_T.multiply(0.00001)
    #df["headMotT360"] = np.where(df["headMotT360"] > 360, df["headMotT360"] - 360, df["headMotT360"])
    #df["headMot_TMA3"] = df["headMotT360"].rolling(3).mean()
    # ---headMot_T差分　diff()
    #df["headMotTMA3diff"] = df["headMot_TMA3"].diff()
    # -----------headMot_TMA3とyaw_Tのpeaks 検出して補正値計算-------------------------------
    peaks, _ = find_peaks(df["headMot_TMA3"], distance=8)
    ypeaks, _ = find_peaks(df["yaw_T"], distance=12)
    print(peaks)
    df["headMot_peaks"]=0
    df["yaw_peaks"]=0
    df["yaw_peakPos"]=0
    df["yawHosei"]=0
    headMotmax=[]
    headMotindex=[]
    yawmax=[]
    yawindex=[]
    
    #peaksに近いypeaksを探す
    for i in range(0,len(peaks)-1):
            print("i=",i)
            headMotindex.append(peaks[i])
            crow=peaks[i]
            headMotmax.append(df.loc[crow,"headMot_TMA3"])  
            df.loc[crow,"headMot_peaks"]=headMotmax[i]      
            for j in range(0,len(ypeaks)-1):
                    print("j=",j)
                    if peaks[i]>ypeaks[j] and peaks[i]<ypeaks[j+1] :                        
                            maesa=peaks[i]-ypeaks[j+1]
                            atosa=ypeaks[j+1]-peaks[i]
                            if maesa<=atosa:
                                df.loc[ypeaks[j],"yaw_peakPos"]=100#yawpeak位置マーク
                                yawmax.append(df.loc[ypeaks[j],"yaw_T"])
                                yawindex.append(ypeaks[j])
                                df.loc[crow,"yaw_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる                   
                            else:
                                df.loc[ypeaks[j+1],"yaw_peakPos"]=100#yawpeak位置マーク
                                yawmax.append(df.loc[ypeaks[j+1],"yaw_T"])
                                yawindex.append(ypeaks[j+1])
                                df.loc[crow,"yaw_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
                    elif peaks[i]==ypeaks[j] or peaks[i]==ypeaks[j+1]:#headMot peakとyaw peakが等しい時
                            df.loc[ypeaks[j],"yaw_peakPos"]=100  
                            df.loc[crow,"yaw_peaks"]=df.loc[ypeaks[j],"yaw_T"]
                            yawmax.append(df.loc[ypeaks[j],"yaw_T"])
                            yawindex.append(ypeaks[j])

    print("headMotmax:",headMotmax)  
    print("headMotindex:",headMotindex)
    print("yawmax:",yawmax)  
    print("yawindex:",yawindex)
    #補正値で埋める
    df["yawHosei"]=0
    df["yasH"]=0
    for i in range(0,len(headMotindex)-1):
        for j in range(yawindex[i],yawindex[i+1]):
            df.loc[j,"yawHosei"]=headMotmax[i]-yawmax[i]
            df.loc[j,"yawH"]= df.loc[j,"yawHosei"]+df.loc[j,"yaw_T"]
    #print(df)
   


    return df
def getnutral4(df):
    npt = []  # Nutral Point index No
    npx = []  # Nutral Point relPosE
    npy = []  # Nutral Point relPosN
    npt_1 = 1
    nptc = 0
    df=additions4(df)
    print
    for i in range(1, len(df.index) - 1):
        ar = df.loc[i, "roll"]
        ar_1 = df.loc[i - 1, "roll"]
        if ar * ar_1 < 0 and i - npt_1 > 3:
            npt_1 = i
            npt.append(i)
            npx.append(df.loc[i, "relPosE_B"])
            npy.append(df.loc[i, "relPosN_B"])
            nptc += 1
            # ("Nutral Point No=",nptc,"index=",i,"npx=",df.loc[i,"relPosE_B"],"npy=",df.loc[i, "relPosN_B"])

    return npt, npx, npy

# ======================================================================================
# ===================Turn Cutter =======================================================
# ================NutralPoint　from Roll　ZeroCross　
# ================Turn parameter =====================================================
def Yaw2Rover(dfrtk, dfbno):
    # yaw角から1mMovingBase Rover座標に変換
    print()


# df_Turn
def turncut(dfdata):
    thead = ["height_B", "gSpeed_B", "headMot_B", "pDOP_B", "relPosN_B", "relPosE_B", "relPosD_B", "relPosLength_B",
             "Heading360", "headMotMA3", "yaw", "pitch", "roll", "ax", "ay", "az", "H-Ysa360", "skid", "Hfp"]
    descr = ["count", "mean", "std", "min", "25%", "50%", "75%", "max"]
    r0 = getnutral(dfdata)
    npt = r0[0]
    npx = r0[1]
    npy = r0[2]
    desc = []
    dft = pd.DataFrame()
    dftd = pd.DataFrame()
    dftcon = pd.DataFrame()
    dftdcon = pd.DataFrame()
    for i in range(1, len(npt) - 1):
        # print("i=",i,"start=",npt[i],"stop=",npt[i+1])
        dft = dfdata.loc[npt[i]:npt[i + 1], thead]
        dftd = dft.describe()
        dft.insert(0, 'turnNo', i)
        dftd.insert(0, 'turnNo', i)
        dftd.insert(0, "describe", descr)
        # print(dftd)
        dftcon = pd.concat([dftcon, dft])
        dftdcon = pd.concat([dftdcon, dftd])

    # print(dftcon)
    # print(dftdcon)
    dftcon.to_csv(r'C:/RTK_Log/Turn_' + basename, index=False)  # CSV save
    dftdcon.to_csv(r'C:/RTK_Log/TurnDesc_' + basename, index=False)  # CSV save


def plotvector3(df):#v キーで起動　RBRNファイル読み込み済み
    print("-----------------plotvector3----------------------------")
    cntj = 1
    # dfzero = df.query('diffzero >0')
    # zeropoint = dfzero.index
    gn = getnutral(df)  # Nutral Point 計算
    zeropoint = gn[0]  # NutralPoint index LIST
    npx = gn[1]  # NutaralPoint X座標LIST
    npy = gn[2]  # NutralPoint Y座標LIST
    # print("zeropoint=",zeropoint)
    # print("npx=",npx)
    # print("npy=",npy)
    # =============================================
    st = ""
    while st != 'e':
        tn = []
        x = []
        y = []
        xp = []
        yp = []
        xpr = []
        ypr = []
        xprd = []
        yprd = []
        hmt=[]
        hdR=[]
        lcorr=[]
        vmx = []
        vmy = []
        hmx = []
        hmy = []
        gsp = []
        skid = []
        high = []
        edge = []
        # 描画範囲　NutralPoint 番号指定　開始ー終了
        # for i in range(0,len(zeropoint)):
        # print("i=",i,"index=",zeropoint[i]
        print("範囲1-",len(zeropoint))
        st = input("範囲 indexNoを入力してください: Exit=>'e'")
        tn = st.split('-')
        # print("tn=",tn)
        # for i in range(1, len(zeropoint) - 1):
        stindex = zeropoint[int(tn[0])]  # ターン開始result.index番号
        stpindex = zeropoint[int(tn[1])]  # ターン終了result.index番号
        # print("plot start=",stindex,"stop=",stpindex)
        # Turn Slope計算
        xd = df.loc[stpindex, "relPosE_B"] - df.loc[stindex, "relPosE_B"]
        yd = df.loc[stpindex, "relPosN_B"] - df.loc[stindex, "relPosN_B"]
        hd = df.loc[stpindex, "height_B"] - df.loc[stindex, "height_B"]
        hd = hd * 0.1
        # figタイトル用　平均　標準偏差
        # skid 平均　正負にわける
        sum0 = 0
        sum1 = 0
        skid0 = []
        skid1 = []
        spds = []
        spdsa = []
        spdsv = []
        itowB=[]
        for n in range(stindex, stpindex):  # 指定範囲の統計値
            spds.append(df.loc[n, "gSpeed_B"])
            itowB.append(df.loc[n,"iTOW_B"])
            c = df.loc[n, "skid"]
            if c < 0:
                # sum0=sum0+df.loc[n,"skid"]
                skid0.append(c)
            else:
                skid1.append(c)
        """
        spdsa = statistics.mean(spds)
        spdsa = spdsa * 0.000001 * 3600
        spdsastr = "{:.1f}".format(spdsa)
        spdsv = statistics.pstdev(spds) * 2
        spdsv = spdsv * 0.000001 * 3600
        spdsvstr = "{:.1f}".format(spdsv)
        skida0 = statistics.mean(skid0)
        skida1 = statistics.mean(skid1)
        skidv0 = statistics.pstdev(skid0) * 2
        skidv1 = statistics.pstdev(skid1) * 2
        skidastr0 = "{:.1f}".format(skida0)
        skidastr1 = "{:.1f}".format(skida1)
        skidvstr0 = "{:.1f}".format(skidv0)
        skidvstr1 = "{:.1f}".format(skidv1)
        """
        turndist = math.sqrt(xd ** 2 + yd ** 2)
        turnslope = math.degrees(math.atan(hd / turndist))
        turnslopestr = "{:.1f}".format(turnslope)
        rng = stpindex - stindex
        # print(stindex, "-", stpindex)
        x0 = df.loc[stindex, "relPosE_B"]  # Baseスタート位置X
        y0 = df.loc[stindex, "relPosN_B"]  # Baseスタート位置Y
        xr0 = df.loc[stindex, "relPosE_R"] + x0  # Roverスタート位置X
        yr0 = df.loc[stindex, "relPosN_R"] + y0  # Roverスタート位置Y
        # print("x0=",x0,"y0=",y0)
        for j in range(stindex, stpindex):
            # print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"])
            rPNB = df.loc[j, "relPosE_B"]  # Base relE
            rPEB = df.loc[j, "relPosN_B"]  # Base relN
            dpxB = int(rPNB - x0)  # Baseターン座標X
            dpyB = int(rPEB - y0)  # Baseターン座標y
            xp.append(dpxB)  # Baseプロット座標LIST xp
            yp.append(dpyB)  # Baseプロット座標LIST yp
            # if j%5==0:
            rPER = df.loc[j, "relPosE_R"]  # Rover relE
            rPNR = df.loc[j, "relPosN_R"]  # Rover relN
            dpxR = rPER + dpxB  # Roverターン座標X
            dpyR = rPNR + dpyB  # Roverターン座標Y
            xpr.append(dpxR)  # Roverプロット座標　xpr
            ypr.append(dpyR)  # Roverプロット座標　ypr
            hmt.append(df.loc[j,"headMot_B"]*0.00001)
            hdR.append(df.loc[j,"Heading360"])
            if 'linecorr' in df.columns :
                lcorr.append(df.loc[j,"linecorr"])
            # print("j=", j, "rPNB,rPEB=", rPNB, rPEB,"rPER,rPNR=",rPER,rPNR, "dpxB,dpyB=", dpxB, dpyB,"dpxR,dpyR=",dpxR,dpyR)
            # print("j=",j,"xr,yr",dxr,dyr)
            # print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj])
            # print(x)
            # print(y)
            # headMot
            vm = arrowmake(df, j)
            vmx.append(vm[0])
            vmy.append(vm[1])
            hmx.append(vm[2])
            hmy.append(vm[3])
            gsp.append(vm[4])
            skid.append(vm[5])
            high.append(vm[6])
            edge.append(vm[7])
            # print("vmx=", vmx, "vmy=", vmy)
            # print("hmx=", hmx, "hmy=", hmy)
            # print("gsp=", gsp)
            # print("skid=", skid)
            # print("high=", high)
            # print("edge=", edge)
            cntj += 1
        # Fig plot
        fig, ax = plt.subplots(figsize=(12, 10))
        tstr = (
                    "\nTurnSlope=" + turnslopestr + "deg" )
        ttl = "TurnNo" + tn[0] + "-" + tn[1] + "indexNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + tstr
        fig.suptitle(ttl)
        ax.set_aspect('equal')
        # dataの最大最小を得る
        print("[xp]=", xp)
        print("[yp]=", yp)
        xpmax = max(xp)
        xpmin = min(xp)
        ypmax = max(yp)
        ypmin = min(yp)
        print(xpmax, xpmin, ypmax, ypmin)
        xprmax = max(xpr)
        xprmin = min(xpr)
        yprmax = max(ypr)
        yprmin = min(ypr)
        print(xprmax, xprmin, yprmax, yprmin)
        # max
        if xpmax > xprmax:
            xmax = xpmax
        else:
            xmax = xprmax
        if ypmax > yprmax:
            ymax = ypmax
        else:
            ymax = yprmax
        # min
        if xpmin < xprmin:
            xmin = xpmin
        else:
            xmin = xprmin
        if ypmin < yprmin:
            ymin = ypmin
        else:
            ymin = yprmin

        # ax.set_xlim(xpmin, xpmax)
        # ax.set_ylim(ypmin, ypmax)
        xmin=int(xmin*1.2)
        xmax=int(xmax*1.2)
        ymin=int(ymin*1.2)
        ymax=int(ymax*1.2)
        print(xmin, xmax, ymin, ymax)
        v = [xmin, xmax, ymin, ymax]
        ax.axis(v)
        # ax.scatter(xp, yp, s=40, c="black")
        ax.plot(xp, yp,color='red')
        ax.plot(xpr, ypr,color='green')
        # 描画範囲を取得する。
        # xmin, xmax = ax.get_xlim()
        # ymin, ymax = ax.get_ylim()
        # print(f"x: [{xmin}, {max}], y: [{ymin}, {ymax}]")
        ax.plot([xp, xpr], [yp, ypr], color="black")
        # ax.quiver(x, y, vmx, vmy, color="red", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
        # headaxislength=1.3)
        # ax.quiver(x, y, hmx, hmy, color="blue", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
        # headaxislength=1.3)
       
    
        # 指定した位置に注釈入れる
        if stpindex - stindex < 500:
            for k in range(1, rng):
                if skid[k] > 0:
                    xt = xp[k] 
                    yt = yp[k] 
                elif skid[k] <= 0:
                    xt = xp[k] 
                    yt = yp[k] 
                # gspstr=str(gsp[k])
                gspstr = "{:.1f}".format(gsp[k])
                # gspstr=gspstr+"kmh"
                skidstr = "{:.1f}".format(skid[k])
                highstr = "{:.2f}".format(high[k] - high[0])
                edgestr = "{:.1f}".format(edge[k])
                itowBstr="{:.0f}".format(itowB[k])
                hmtstr="{:.0f}".format(hmt[k])
                hdRstr="{:.0f}".format(hdR[k])
                # linecorrがある場合
                if 'linecorr' in df.columns :
                    lcorrstr="{:.4f}".format(lcorr[k])
                else:
                    lcorrstr=""
                txtstr = "[itow"+itowBstr+"]\nspd" + gspstr + "kmh" + "\nskid" + skidstr + "deg"+"hmt"+hmtstr+"-hdR"+hdRstr+"deg" + "\nDrop" + highstr + "m" + "\nEdge" + edgestr+"deg,lcorr"+lcorrstr
                #ax.text(xt, yt, txtstr, fontsize="x-small")
               
                ax.annotate(txtstr, xy=(xt, yt),xytext=(xt+30,yt), size=6,color="black", arrowprops=dict())
               
        # matplotlib.pyplot.text(x, y, s, fontdict=None, withdash= < deprecatedparameter >, ** kwargs)
        plt.show()
    input("ANY key return")
#-BootsOn RTKプロットDFをresultBRBNへ追加



def RTK2BRBN(df0,df1):
    print(df0)
    print(df1)
    #df0 resultBRBNとdf1 df_BTplotの先頭itowと長さを獲る
    startitowBR=int(df0.loc[0,"iTOW_B"])
    startitowBT=int(df1.loc[0,"iTOW_T"])
    btmod=startitowBT % 120
    startitowBT=startitowBT+btmod
    BRlen=len(df0)
    BTlen=len(df1)
    isa=startitowBR-startitowBT
    list_BTplot=[]
    if isa<=0:#BRBNが先にログ開始した場合右RTKが遅れる
        brindxl= list(df0.reset_index().query('iTOW_B == @startitowBT').index)
        brindx=brindxl[0]
        print(brindx)
        btcoln=len(df1.columns)
        list_BTplot = [['0']*btcoln] * brindx # 上のデータゼロ埋めてlist_BTplot作成
        print(list_BTplot)
        btlen = len(df1)
        for i in range(0, btlen - 1):  # 中間にdf_BTplotをlistに追加
            r = df1.iloc[i, :]
            list_BTplot.append(r)#df1をlistにコピー
        sn = brindx + len(df1)
        rn = len(df0)
        print("Upper:list_BTplot=", len(list_BTplot))
        print("for sn=", sn, "rn=", rn, "result len=", len(df0))
        for i in range(sn, rn + 1):  # 下のデータ埋め
            l0 = [['0']*btcoln]
            list_BTplot.append(l0)
        print("list_BTplot=", len(list_BTplot))
        df2 = pd.DataFrame(list_BTplot, columns=df1.columns)
        print("df2=",df2)
    else:#右RTKが先にログ開始した場合右BRBNが遅れる
        btindx = df1[df1['iTOW_T'] == startitowBR].index
        print(startitowBR,startitowBT,"BR-BT=",startitowBR-startitowBT,BRlen,BTlen)

        print("Okure df2=",df2)
    # resultBRBNとdf2をconcate
    df3= pd.concat([df0, df2], axis=1)
    print(df3)
    return df3
# |||||||||||||||||||||||||||||||||readMBFIle END|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
# [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]


def itow120df(df):  # dfは、先頭がiTOW
    itow120list = []
    for i in range(1, len(df) - 1):
        mod = int(df.iloc[i, 0]) % 120
        if mod < 10:
            itow120list.append(df.iloc[i, :])
            print("itow120df_Check:",i,">bnitow=",df.iloc[i,0])
            #print(itow120list)
    df_itow120 = pd.DataFrame(itow120list)
    #print("df_itwo120=",df_itow120)
    return df_itow120


def bnitowsort(df):
    firstbnitow = 0
    i = 0
    dflen=len(df)
    while i<dflen-1 and firstbnitow<10000:
        if int(df.loc[i, "bnitow"]) < 100000 :
            print("DROP i=",i,"bnioitow=",df.loc[i, "bnitow"])
            df=df.drop(index=i)
        else:  
            firstbnitow = int(df.loc[i, "bnitow"])
        i += 1
        #dflen=len(df)
        #if i%1000==0:
        print("bnoirowsort_i=",i)
    #firstbnitowから10stepで埋める
    #print(df.head(2))
    df=df.reset_index(drop=True)
    df.to_csv(r'C:/RTK_Log/bnitow10_dropped_' + basename)  # CSV save
    #df["bnitow"].apply(lambda x:x+10)
    #df['bnitow'] = range(1, len(df.index) + 1)
   
    for i in range(1, len(df)):
        df.loc[i, "bnitow"] = firstbnitow + 10 * i
        #print("bnosorted bnitow[",i,"]=",df.loc[i,"bnitow"])
    #print(df)
    print("--------bnitow File sorted---------dflen=",len(df))
    df.to_csv(r'C:/RTK_Log/bnitow10_sorted' + basename)  # CSV save
    return df


def bnitowRe(df):  # bnitowの異常値を修正
    # 0,1,2行ゼロ処理
    czeroflag = 0
    for i in range(0, len(df) - 1):
        if int(df.loc[i, "bnitow"]) == 0:
            j = i
            while int(df.loc[j, "bnitow"]) == 0:
                print("==0:j=", j, int(df.loc[j, "bnitow"]))
                j += 1
            for k in range(j, i, -1):  # j+1のbnitow値から-10decriment
                df.loc[k - 1, "bnitow"] = int(df.loc[k, "bnitow"]) - 10
                print("==0", df.loc[k - 1, "bnitow"])
        elif int(df.loc[i, "bnitow"]) > 10:
            j = i
            while int(df.loc[j, "bnitow"]) > 10:
                print(">10:j=", j, int(df.loc[j, "bnitow"]))
                j += 1
            for k in range(j, i, -1):  # j+1のbnitow値から-10decriment
                df.loc[k - 1, "bnitow"] = int(df.loc[k, "bnitow"]) - 10
                print(">10:", df.loc[k - 1, "bnitow"])
    print("def Repaired=", df)

    return df
def readBRBNTnutral(df):#headMot_peaksでNutralPointを読んでindex をlistで返す
    
    npt = []  # Nutral Point index No
    
    for i in range(0, len(df.index) - 1):
        if df.loc[i,"headMot_peaks"]!=0:
            print(i,"=================:headMot_peaks=",df.loc[i,"headMot_peaks"])
            npt.append(i)
        #else:
            #print(i,":headMot_peaks=",df.loc[i,"headMot_peaks"])
        
    return npt
def comnt(df,sti,stpi):
    print()
   
    BRstrs=[]#BR戻しLIST
    BTstrs=[]#BT戻しLIST
    for j in range(sti, stpi):
            hmotdeg = df.loc[j, "headMot360"]
            hmotdegT= df.loc[j, "headMotT360"]
            headdeg = df.loc[j, "Heading360"]
            headdegT = df.loc[j, "yaw_T"]
            gsp = df.loc[j, "gSpeed_B"] * 0.001 * 3600 * 0.001
            gspT = df.loc[j, "gSpeed_T"] * 0.001 * 3600 * 0.001
            skid = hmotdeg - headdeg
            skidT = hmotdegT - headdegT
            high = df.loc[j, "height_B"] * 0.001
            high0= df.loc[0, "height_B"] * 0.001
            highT = df.loc[j, "height_T"] * 0.001
            high0T = df.loc[0, "height_T"] * 0.001
            edge = df.loc[j, "roll"]
            edgeT = df.loc[j, "roll_T"]
            itowB=df.loc[j,"iTOW_B"]
            itowT=df.loc[j,"iTOW_T"]

            gspstr = "{:.1f}".format(gsp)
            skidstr = "{:.1f}".format(skid)
            highstr = "{:.2f}".format(high - high0)
            edgestr = "{:.1f}".format(edge)
            itowBstr=str(itowB)#"{:.0f}".format(itowB)

            gspstrT = "{:.1f}".format(gsp)
            skidstrT = "{:.1f}".format(skidT)
            highstrT = "{:.2f}".format(highT - high0T)
            edgestrT = "{:.1f}".format(edgeT)
            itowBstrT=str(itowT)#"{:.0f}".format(itowT)

            BRstr = "[itow"+itowBstr+"]\nspd" + gspstr + "kmh" + "\nskid" + skidstr +"deg" + "\nDrop" + highstr + "m" + "\nEdge" + edgestr
            BTstr = "[itowT"+itowBstrT+"]\nspdT" + gspstrT + "kmh" + "\nskidT" + skidstrT +"deg" + "\nDropT" + highstrT + "m" + "\nEdgeT" + edgestrT
            
            BRstrs.append(BRstr)
            BTstrs.append(BTstr)

    return BRstrs,BTstrs       
            
    
# [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main END ]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
# ********************************************RL PLOT plotvector4(df_BRBNT)**********************************************************
# ************************************* df_BRBNTをわたされてRL描画*************************************************
def plotvector4(df):
    print("-------------------plotvector4-------------------------------")
    cntj = 1
    # dfzero = df.query('diffzero >0')
    # zeropoint = dfzero.index
    #if bnOK==0:
    #gn = getnutral4(df)  # Nutral Point 計算
    zeropoint=readBRBNTnutral(df)#zeropointにNutralPointのindex収納
    #zeropoint = gn[0]  # NutralPoint index ゼロ番
    #npx = gn[1]  # NutaralPoint X座標LIST
    #npy = gn[2]  # NutralPoint Y座標LIST
    print("zeropoint=",zeropoint)
    # print("npx=",npx)
    # print("npy=",npy)
    # =============================================
    st = ""
    while st != 'e':
        tn = []
        x = []
        y = []
        xp = []
        xpbt=[]
        yp = []
        ypbt=[]
        xpr = []
        xprbt=[]
        ypr = []
        yprbt=[]
        xprd = []
        xprdbt=[]
        yprd = []
        yprdbt=[]
        vmx = []
        vmy = []
        hmx = []
        hmy = []
        gsp = []
        gspbt=[]
        skid = []
        skidbt=[]
        high = []
        edge = []
        edgebt=[]
        lcorr=[]
        # 描画範囲　NutralPoint 番号指定　開始ー終了
        # for i in range(0,len(zeropoint)):
        # print("i=",i,"index=",zeropoint[i])
        print("範囲1-",len(zeropoint))
        st = input(" indexNoを入力してください: Exit=>'e'")
        tn = st.split('-')
        # print("tn=",tn)
        # for i in range(1, len(zeropoint) - 1):
        stindex = zeropoint[int(tn[0])]  # ターン開始result.index番号
        stpindex = zeropoint[int(tn[1])]  # ターン終了result.index番号
        # print("plot start=",stindex,"stop=",stpindex)
        # Turn Slope計算
        xd = df.loc[stpindex, "relPosE_B"] - df.loc[stindex, "relPosE_B"]
        yd = df.loc[stpindex, "relPosN_B"] - df.loc[stindex, "relPosN_B"]
        hd = df.loc[stpindex, "height_B"] - df.loc[stindex, "height_B"]
        hd = hd * 0.1
        # figタイトル用　平均　標準偏差
        # skid 平均　正負にわける
        sum0 = 0
        sum1 = 0
        skid0 = []
        skid1 = []
        spds = []
        spdsa = []
        spdsv = []
        itowB=[]
        for n in range(stindex, stpindex):  # 指定範囲の統計値
            spds.append(df.loc[n, "gSpeed_B"])
            c = df.loc[n, "skid"]
            if c < 0:
                # sum0=sum0+df.loc[n,"skid"]
                skid0.append(c)
            else:
                skid1.append(c)
        spdsa = statistics.mean(spds)
        spdsa = spdsa * 0.000001 * 3600
        spdsastr = "{:.1f}".format(spdsa)
        spdsv = statistics.pstdev(spds) * 2
        spdsv = spdsv * 0.000001 * 3600
        spdsvstr = "{:.1f}".format(spdsv)
        #skida0 = statistics.mean(skid0)
        #skida1 = statistics.mean(skid1)
        #skidv0 = statistics.pstdev(skid0) * 2
        #skidv1 = statistics.pstdev(skid1) * 2
        #skidastr0 = "{:.1f}".format(skida0)
        #skidastr1 = "{:.1f}".format(skida1)
        #skidvstr0 = "{:.1f}".format(skidv0)
        #skidvstr1 = "{:.1f}".format(skidv1)
      
        turndist = math.sqrt(xd ** 2 + yd ** 2)
        turnslope = math.degrees(math.atan(hd / turndist))
        turnslopestr = "{:.1f}".format(turnslope)
        rng = stpindex - stindex
        # print(stindex, "-", stpindex)
        x0 = df.loc[stindex, "relPosE_B"]  # Baseスタート位置X
        y0 = df.loc[stindex, "relPosN_B"]  # Baseスタート位置Y
        xr0 = df.loc[stindex, "relPosE_R"] + x0  # Roverスタート位置X
        yr0 = df.loc[stindex, "relPosN_R"] + y0  # Roverスタート位置Y
        #BT座標 BASE:xbtb,ybtb  ROVER:xbtr ybtr

        # print("x0=",x0,"y0=",y0)
        for j in range(stindex, stpindex):
            itowB.append(df.loc[j,"iTOW_B"])
            #----------SkiMB座標-------------------
            # print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"])
            rPEB = df.loc[j, "relPosE_B"]  # Base relE　基線座標
            rPNB = df.loc[j, "relPosN_B"]  # Base relN　基線座標
            dpxB = int(rPEB - x0)  # Baseターンプロット座標X
            dpyB = int(rPNB - y0)  # Baseターンプロット座標y
            xp.append(dpxB)  # Baseプロット座標LIST xp
            yp.append(dpyB)  # Baseプロット座標LIST yp
            # if j%5==0:
            rPER = df.loc[j, "relPosE_R"]  # Rover relE　MB座標
            rPNR = df.loc[j, "relPosN_R"]  # Rover relN　MB座標
            dpxR = rPER + dpxB  # Roverターンプロット座標X
            dpyR = rPNR + dpyB  # Roverターンプロット座標Y
            xpr.append(dpxR)  # Roverプロット座標LIST　xpr
            ypr.append(dpyR)  # Roverプロット座標LIST　ypr
            #-----------BT座標------------------------
            rPEB_BT=df.loc[j,"rpEB_BT"] #BT Base Y　基線座標
            rPNB_BT=df.loc[j,"rpNB_BT"] #BT Base X　基線座標
            dpxB_BT=int(rPEB_BT-x0)#Base　プロット座標X　原点x0はSki　Base（前）
            dpyB_BT=int(rPNB_BT-y0)#Base プロット座標Y　原点y0はSki　Base（前）
            xpbt.append(dpxB_BT)  # Baseプロット座標LIST xpbt
            ypbt.append(dpyB_BT)  # Baseプロット座標LIST ypbtr
            #rover BT
            rPER_BT=df.loc[j,"rpER_BT"] #BT Rover X　基線座標
            rPNR_BT=df.loc[j,"rpNR_BT"] #BT Rover Y　基線座標
            dpxR_BT=int(rPER_BT-x0)#Rover　プロット座標X　原点x0はSki　Base（前）
            dpyR_BT=int(rPNR_BT-y0)#Rover　プロット座標X　原点x0はSki　Base（前）
            xprbt.append(dpxR_BT)# Roverプロット座標LIST xprbt
            yprbt.append(dpyR_BT) # Roverプロット座標LIST ypbt

            # print("j=", j, "rPNB,rPEB=", rPNB, rPEB,"rPER,rPNR=",rPER,rPNR, "dpxB,dpyB=", dpxB, dpyB,"dpxR,dpyR=",dpxR,dpyR)
            # print("j=",j,"xr,yr",dxr,dyr)
            # print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj])
            # print(x)
            # print(y)
            # headMot
            vm = arrowmake(df, j)
            vmx.append(vm[0])
            vmy.append(vm[1])
            hmx.append(vm[2])
            hmy.append(vm[3])
            gsp.append(vm[4])
            skid.append(vm[5])
            high.append(vm[6])
            edge.append(vm[7])
            # print("vmx=", vmx, "vmy=", vmy)
            # print("hmx=", hmx, "hmy=", hmy)
            # print("gsp=", gsp)
            # print("skid=", skid)
            # print("high=", high)
            # print("edge=", edge)
            cntj += 1
        # Fig plot
        fig, ax = plt.subplots(figsize=(12, 10))
        tstr = (
                    "\nTurnSlope=" + turnslopestr + "deg" + ",AvSpd=" + spdsastr + "±" + spdsvstr + "kmh" )
                
        ttl = "TurnNo" + tn[0] + "-" + tn[1] + "indexNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + tstr
        fig.suptitle(ttl)
        ax.set_aspect('equal')
        # dataの最大最小を得る
        print("[xp]=", xp)
        print("[yp]=", yp)
        xpmax = max(xp)
        xpmin = min(xp)
        ypmax = max(yp)
        ypmin = min(yp)
        print(xpmax, xpmin, ypmax, ypmin)
        xprmax = max(xpr)
        xprmin = min(xpr)
        yprmax = max(ypr)
        yprmin = min(ypr)
        print(xprmax, xprmin, yprmax, yprmin)
        # max
        if xpmax > xprmax:
            xmax = xpmax
        else:
            xmax = xprmax
        if ypmax > yprmax:
            ymax = ypmax
        else:
            ymax = yprmax
        # min
        if xpmin < xprmin:
            xmin = xpmin
        else:
            xmin = xprmin
        if ypmin < yprmin:
            ymin = ypmin
        else:
            ymin = yprmin

        # ax.set_xlim(xpmin, xpmax)
        # ax.set_ylim(ypmin, ypmax)
        print(xmin, xmax, ymin, ymax)
        m=100
        v = [xmin+m, xmax+m, ymin+m, ymax+m]
        ax.axis(v)
        # ax.scatter(xp, yp, s=40, c="black")
        ax.plot(xp, yp,color='red')
        ax.plot(xpr, ypr,color='green')
        ax.plot(xpbt, ypbt,color='magenta')
        ax.plot(xprbt, yprbt,color='aqua')

        # 描画範囲を取得する。
        # xmin, xmax = ax.get_xlim()
        # ymin, ymax = ax.get_ylim()
        # print(f"x: [{xmin}, {max}], y: [{ymin}, {ymax}]")
        c=0
        for j in range(stindex, stpindex-1):
            c+=1
            if df.loc[j,"iTOW_B"]%120==0:
                ax.plot([xp[c], xpr[c]], [yp[c], ypr[c]], color="black", linewidth=3)
                #ax.plot([xpbt[c-1], xprbt[c-1]], [ypbt[c-1], yprbt[c-1]], color="blue",linewidth=3)
                ax.plot([xpbt[c], xprbt[c]], [ypbt[c], yprbt[c]], color="blue",linewidth=3)
        # ax.quiver(x, y, vmx, vmy, color="red", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
        # headaxislength=1.3)
        # ax.quiver(x, y, hmx, hmy, color="blue", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
        # headaxislength=1.3

        # 指定した位置に注釈入れる
        if stpindex - stindex < 500:
            for k in range(1, rng):
                if skid[k] > 0:
                    xt = xp[k] 
                    yt = yp[k] 
                elif skid[k] <= 0:
                    xt = xp[k] 
                    yt = yp[k] 
                xtT=xpbt[k]
                ytT=ypbt[k]

                # gspstr=str(gsp[k])
                gspstr = "{:.1f}".format(gsp[k])
                # gspstr=gspstr+"kmh"
                skidstr = "{:.1f}".format(skid[k])
                highstr = "{:.2f}".format(high[k] - high[0])
                edgestr = "{:.1f}".format(edge[k])
                itowBstr="{:.0f}".format(itowB[k])
                hmtstr="0"#"{:.0f}".format(hmt[k])
                hdRstr="0"#"{:.0f}".format(hdR[k])
                # linecorrがある場合
                if 'linecorr' in df.columns :
                    lcorrstr="0"#"{:.4f}".format(lcorr[k])
                else:
                    lcorrstr=""
                txtstr = "[itow"+itowBstr+"]\nspd" + gspstr + "kmh" + "\nskid" + skidstr + "deg"+"hmt"+hmtstr+"-hdR"+hdRstr+"deg" + "\nDrop" + highstr + "m" + "\nEdge" + edgestr+"deg,lcorr"+lcorrstr
                #ax.text(xt, yt, txtstr, fontsize="x-small")
                

                ax.annotate(txtstr, xy=(xt, yt),xytext=(xt+60,yt), size=6,color="black", arrowprops=dict())
                #
                rtn= comnt(df,stindex,stpindex)
                BRstr=[]
                BRstr=rtn[0]
                BTstr=[]
                BTstr=rtn[1]
                ax.annotate(BTstr[k], xy=(xtT, ytT),xytext=(xtT-140,ytT), size=6,color="black", arrowprops=dict())
             
        plt.show()
        """
        # 指定した位置に注釈入れる
        if stpindex - stindex < 20:
            for k in range(1, rng):
                if skid[k] > 0:
                    xt = x[k] + 40
                    yt = y[k] + 60
                elif skid[k] <= 0:
                    xt = x[k] + 100
                    yt = y[k] + 100
                # gspstr=str(gsp[k])
                gspstr = "{:.1f}".format(gsp[k])
                # gspstr=gspstr+"kmh"
                skidstr = "{:.1f}".format(skid[k])
                highstr = "{:.2f}".format(high[k] - high[0])
                edgestr = "{:.1f}".format(edge[k])
                txtstr = "[spd=" + gspstr + "kmh" + "\nskid=" + skidstr + "deg" + "\nDrop=" + highstr + "m" + "\nEdge" + edgestr
                ax.text(xt, yt, txtstr, fontsize="small")
        # matplotlib.pyplot.text(x, y, s, fontdict=None, withdash= < deprecatedparameter >, ** kwargs)
        """
    input("ANY key return")

def sqrtv(x,y):
    v=math.sqrt(x**2+y**2)
    return v
#*******************BNO10msec加速度ベクトルを２回積分で距離をだす初期値はgSpeedを与える************
#BRBNファイルとBNOファイルを読み込む
def bnointegral():
        print("**************************BNO Integral  Choose BNOFile & BRBN FILE*************")
        fok=False
        bnof=False
        brf=False
        while fok==False:
            
            root = tkinter.Tk()
            root.withdraw()
            iDir = r'C:/RTK_Log/'
            fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
            filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
            # filenameからbasenameをとりだしてファイル種類区別
            basename = os.path.basename(filename)
            if "BRBN" not in basename and "BNO" in basename:
                bnof=True
                basename_BNO=basename
                list_BNO=[]
                head1 = list_BNO
                BNOflag = True
                #df_BNO = pd.read_csv(filename, names=head1, low_memory=True)
                df_BNO = pd.read_csv(filename, low_memory=True)
                print("BNO File read=",filename)
                BNOitow = int(df_BNO.loc[0, "bnitow"])
                print("BNOitow=",BNOitow)
                
            elif "BRBN" in basename:
                brf=True
                basename_BRBN=basename
                brbnOK=1#BRBNファイルでSKI MBプロットvectorplot3
                bnOK=0
                df_BRBN = pd.read_csv(filename, low_memory=True)
                print("df_BRBN read",filename)
                BRBNitow = int(df_BRBN.iloc[0, 1])
                print("BRBNitow=", BRBNitow)
                BRBNflag=True
                
            fok=bnof and brf
            print("READ:BNO File=",bnof,"BRBN FIle=",brf)    
        dflen=len(df_BNO)
        print("df_BNO=",dflen,":[",df_BNO.loc[0,"bnitow"],"-",df_BNO.loc[dflen-1,"bnitow"],"]",basename_BNO)
        dflen=len(df_BRBN)
        print("df_BRBN=",dflen,":[",df_BRBN.loc[0,"iTOW_B"],"-",df_BRBN.loc[dflen-1,"iTOW_B"],"]",basename_BRBN)
        pd.set_option('display.max_columns', 40)
        #積分準備　df_BNOに積分に必要なデータをdf_BRBNから追加する
        #[1]itow_Bの積分範囲 [2]
        #積分用データ列追加
        df_BNO["vx"]=0#x速度成分　加速度1回積分
        df_BNO["vy"]=0#y速度成分　加速度1回積分
        df_BNO["dx"]=0#x距離成分　加速度2回積分
        df_BNO["dy"]=0#y距離成分　加速度2回積分
        df_BNO["Va"]=0#xy加速度ベクトル
        df_BNO["Vv"]=0#xy速度ベクトル 1回積分
        df_BNO["Vd"]=0#xy距離ベクトル 2回積分
        
        #Vaをax ayから計算代入df['D'] = df.apply(lambda row: row['A'] + row['B']
        df_BNO['Va'] = df_BNO.apply(lambda row:math.sqrt(row['ax']**2+row['ay']**2), axis=1)
        #print(df_BNO['Va'])
        #BRBNデータ列追加
        df_BNO["RTKdist"]=0#１周期での移動距離
        df_BNO["HeadingSa"]=0
        df_BNO["gSpeed_B"]=0
        df_BNO["relPosN_B"]=0
        df_BNO["relPosE_B"]=0
        df_BNO["headMot_B"]=0
        df_BNO["relPosHeading_R"]=0
        df_BNO["flags_B"]=0
        df_BNO["diffzero"]=0
        df_BNO["skid"]=0
        df_BNO["linecorr"]=0

        cols=["vx","vy","dx","dy","Va","Vv","Vd","RTKdist","HeadingSa","gSpeed_B","relPosN_B","relPosE_B","headMot_B","relPosHeading_R","flags_B","diffzero","skid","linecorr"]
        df_BNO[cols]=df_BNO[cols].astype("float64")
        #print(df_BNO.dtypes)
        #BNO%120==0とBRGNOのスタート地点探す
        firstflag=False
        bnindex120=[]#bnitow==120のindex list
        bnindex=[]
        df_BNO = df_BNO.round({'bnitow': -1})
        #for i in range(0,len(df_BNO)):
        startflag=False
        i=0#bnoindex
        while startflag==False:          
            for j in range(0,len(df_BRBN)):
                BRBNitow=int(df_BRBN.loc[j,"iTOW_B"])
                if BRBNitow==df_BNO.loc[i,"bnitow"]:
                    startBNOindex=i
                    startBNOitow=df_BNO.loc[i,"bnitow"]
                    startBRBNindex=j
                    startBRBNitow=BRBNitow
                    startflag=True
            
            i+=1
        print("Start BNO:",startBNOindex,startBNOitow,"start BRBN:",startBRBNindex,startBRBNitow)

        k=0
        for i in range(startBRBNindex,len(df_BRBN)-1):            
                brbnitow=int(df_BRBN.loc[i,"iTOW_B"])
                j=startBNOindex+12*k
                bnitow=df_BNO.loc[j,"bnitow"]                                 
                df_BNO.loc[j,"iTOW_B"]=df_BRBN.loc[i,"iTOW_B"]
                df_BNO.loc[j,"gSpeed_B"]=df_BRBN.loc[i,"gSpeed_B"]
                df_BNO.loc[j,"relPosN_B"]=df_BRBN.loc[i,"relPosN_B"]
                df_BNO.loc[j,"relPosE_B"]=df_BRBN.loc[i,"relPosE_B"]
                df_BNO.loc[j,"headMot_B"]=df_BRBN.loc[i,"headMot_B"]*0.00001
                #df_BNO.loc[j,"headMot_B"]=df_BRBN.loc[i,"headMot_B"]
                df_BNO.loc[j,"relPosHeading_R"]=df_BRBN.loc[i,"relPosHeading_R"]*0.00001
                df_BNO.loc[j,"flags_B"]=df_BRBN.loc[i,"flags_B"]
                df_BNO.loc[j,"flags_R"]=df_BRBN.loc[i,"flags_R"]   
                df_BNO.loc[j,"diffzero"]=df_BRBN.loc[i,"diffzero"] 
                df_BNO.loc[j,"skid"]=df_BRBN.loc[i,"skid"] 
                df_BNO.loc[j,"linecorr"]=df_BRBN.loc[i,"linecorr"] 

                #relPosN-E変位 RTKdist
                ndist=df_BRBN.loc[i,"relPosN_B"]-df_BRBN.loc[i-1,"relPosN_B"]
                edist=df_BRBN.loc[i,"relPosE_B"]-df_BRBN.loc[i-1,"relPosE_B"]
                df_BNO.loc[j-1,"RTKdist"]=math.sqrt(ndist**2+edist**2)*0.01
                #relPosHeading差
                head=df_BRBN.loc[i,"relPosHeading_R"]
                head_1=df_BRBN.loc[i-1,"relPosHeading_R"]
                df_BNO.loc[j-1,"HeadingSa"]=(head-head_1)*0.00001
                #Vv初期値
                df_BNO.loc[j,"Vv"]=df_BRBN.loc[i,"gSpeed_B"] *0.001  
                #Vv初期値
                k+=1  
               
                if k>=2:      
                    #----積分用RTK初期値　終端値
                    vinit=df_BNO.loc[j-12,"gSpeed_B"]*0.001
                    vend=df_BNO.loc[j,"gSpeed_B"]*0.001
                    hminit=df_BNO.loc[j-12,"headMot_B"]
                    hmend=df_BNO.loc[j,"headMot_B"]
                    rphinit=df_BNO.loc[j-12,"relPosHeading_R"]
                    rphend=df_BNO.loc[j,"relPosHeading_R"]

                    #積分範囲
                    si=j-12
                    ei=j
                    for n in range(1,12):#積分ループ
                        ax=df_BNO.loc[si+n,"ax"]
                        ax_1=df_BNO.loc[si+n-1,"ax"]
                        ay=df_BNO.loc[si+n,"ay"]
                        ay_1=df_BNO.loc[si+n-1,"ay"]
                        df_BNO.loc[si+n,"vx"]=(ax+ax_1)/2*0.01+df_BNO.loc[si+n-1,'vx']
                        df_BNO.loc[si+n,"vy"]=(ay+ay_1)/2*0.01+df_BNO.loc[si+n-1,'vy']
                        vx=df_BNO.loc[si+n,"vx"]
                        vx_1=df_BNO.loc[si+n-1,"vx"]
                        vy=df_BNO.loc[si+n,"vy"]
                        vy_1=df_BNO.loc[si+n-1,"vy"]
                        df_BNO.loc[si+n,"dx"]=(vx+vx_1)/2*0.01+df_BNO.loc[si+n-1,'vx']
                        df_BNO.loc[si+n,"dy"]=(vy+vy_1)/2*0.01+df_BNO.loc[si+n-1,'vy']
                        #Vector積分
                        va=df_BNO.loc[si+n,"Va"]
                        va_1=df_BNO.loc[si+n-1,"Va"]
                        df_BNO.loc[si+n,"Vv"]=(va+va_1)/2*0.01+df_BNO.loc[si+n-1,'Vv']
                        vv=df_BNO.loc[si+n,"Vv"]
                        vv_1=df_BNO.loc[si+n-1,"Vv"]
                        df_BNO.loc[si+n,"Vd"]=(vv+vv_1)/2*0.01+df_BNO.loc[si+n-1,'Vd']
                        
                 
        df_BNO.to_csv(r'C:/RTK_Log/IntBNO_' + basename)  # CSV save

#直線部検出して"linecorrに各行相関係数記録
def linereg(df,m):#mは奇数
    print("---pandas Line Regression")
    df["linecorr"]=0
    #直線の判定　M点の直線回帰で最大振れ値で規定
    #回帰奇数個mをセンター振り分け
    n=int(m/2)#５個なら２個振り分け
    count=0
    for i in range(m,len(df)-m):
        x=[]
        y=[]
        for j in range(0,m-1):
            x.append(df.loc[j+i,"relPosE_B"])
            y.append(df.loc[j+i,"relPosN_B"])
        # データを用意
        # リストをps.Seriesに変換
        X=pd.Series(x)
        Y=pd.Series(y)
        # pandasを使用してPearson's rを計算
        res=X.corr(Y)   # numpy.float64 に格納される
        df.loc[i+n,"linecorr"]=res#i+n番目に代入５個なら i,i+1,[i+2],i+3,i+4 
        count+=1

    return count
#120msecでheadMotとHeading360が粗いので、BNO10msec座標でNutralPointを細分化して精度向上させる
#def BNHM(dfbr,dfbn):


    
#========================================================================================================
#====================================def main()を最下部で最後に呼び出す====================================
#========================================================================================================
def main():
    global result, basename, df_BNO, BRBNflag
    #  id+PVT33+RELPOSNED 20
    # --------------------------------------------MBUX FILES-----------------------------------------------------------------
    # UBXデータは、BaseとRoverから同形式で出力されたバイナリーをSTA24システムでUBX値に変換してCSVファイル
    # 形式は、Baseは、id列に’B'行で　Roverはid列に’R'行がついている。同じitowが２行となる。　
    # PVT３３個　RELPOSNED 20個　計55個ｘ２=110個整数
    print("main pass through")
    print("hoge")
    #basename=""
    brbnOK:int=0
    btOK:int=0
    bnOK:int=0
    ubxflag:int = 0
    #bnoflag:int = 0
    targetitow = 0
    df_BNO = pd.DataFrame
    list_PVT = ["id", "iTOW", "year", "month", "day", "hour", "min", "sec", "valid", "tAcc", "nano", "fixtye", "flags",
                "flags2", "numSV", "lon", "lat", "height", "hMSL",
                "hAcc", "vAcc", "velN", "velE", "velD", "gSpeed", "headMot", "sAcc", "headAcc", "pDOP", "flags3",
                "reserved1", "headVeh", "magDec", "magAcc", ""]  # 33
    list_BasePVT = [s + '_B' for s in list_PVT]
    list_RoverPVT = [s + '_R' for s in list_PVT]
    list_RELP = ["version", "reserved2", "refStationId", "iTOWR", "relPosN", "relPosE", "relPosD", "relPosLength",
                 "relPosHeading", "relPosHPN",
                 "relPosHPE", "relPosHPD", "relPosHPLength", "accN", "accE", "accD", "accLength", "accHeading",
                 "reserverd4", "flagsR", "a"]
    list_BaseRELP = [s + '_B' for s in list_RELP]
    list_RoverRELP = [s + '_R' for s in list_RELP]

    head0 = list_PVT + list_RELP
    # print(list_BasePVT)
    # print(list_RoverPVT)
    # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    # @@@@@@@@@@@@@@@@@@@@@@@@@@  CONTROL KEY    @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

    v = ""
    cmnt = "[]"
    initflag=0
    while v != "e":
        v=""
        if initflag==1:
            print("Files:" + cmnt)
            #print("BRBNflag=",BRBNflag)
        print("<<<<PUSH KEY::r:read file,h:plot headmot,x:plot xy,v:plot3,g:plot4 e=EXIT>>>> ")
        """
        while True:
            if keyboard.read_key():
                v = keyboard.read_key()
                print("キーが押されました: " + v)
                if v == "e":
                    sys.exit()
                break
        """
        v= input(" keyin =>ENTER")
        #tn = st.split('-')
        # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        initflag=1
        if v == "v":
                plotvector3(resultBRBN)
        if v == "g":
                print("df_BRBNT:",df_BRBNT)
                plotvector4(df_BRBNT)
        if v == "t":
            turncut(result)
        # --------------------------------------------MovingBase BNO FILES-----------------
        if v == "r":
            
            list_BNO = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az", "com1", "com2", "com3"]
            #list_BNO = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az"]
            list_BNO0 = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az"]
            root = tkinter.Tk()
            root.withdraw()

            iDir = r'C:/RTK_Log/'
            fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
            filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
            # filenameからbasenameをとりだしてファイル種類区別
            basename = os.path.basename(filename)
            BRBNflag = "BRBN" in basename
            # ************************************************************************************************************
            # *************************************MovingBase File *******************************************************
            # ************************************************************************************************************
            if  "BRBN" in basename:# BRBNflag == True:
                if "BRBNT" in basename:
                    bnOK=1#BNBRTファイルでRLプロットvectorplot4
                    brbnOK=0
                    df_BRBNT = pd.read_csv(filename, low_memory=True)
                    print("sf_BRBNT:",df_BRBNT)
                    dflen=len(df_BRBNT)
                    print(dflen,":[]",df_BRBNT.loc[0,"iTOW_B"],"-",df_BRBNT.loc[dflen-1,"iTOW_B"],"]")
                    cmnt = "[" + basename + "]" + cmnt
                else:
                    brbnOK=1#BRBNファイルでSKI MBプロットvectorplot3
                    bnOK=0
                    resultBRBN = pd.read_csv(filename, low_memory=True)
                    print(resultBRBN)
                    targetitow = int(resultBRBN.iloc[0, 1])
                    print("targetitow=", targetitow)
                    BRBNflag=True
                    dflen=len(resultBRBN)
                    print("resultBRBN=",dflen,":[",resultBRBN.loc[0,"iTOW_B"],"-",resultBRBN.loc[dflen-1,"iTOW_B"],"]")
                
                    cmnt = "[" + basename + "]" + cmnt
            elif "ubx" in basename and v == "r" and BRBNflag == False:
                ubxflag = 1
                result = readMBFile(filename, basename, head0)
                print("result=", result)
                print(f"result memory usage: {result.memory_usage(deep=True).sum() / 1024 ** 2} MB")
                cmnt = "[" + basename +"]" + cmnt
                v=""
            elif "bno" in basename and v == "r":  # MB BNO File
                v=""
                head1 = list_BNO
                BNOflag = True
                #df_BNO = pd.read_csv(filename, names=head1, low_memory=True)
                df_BNO = pd.read_csv(filename, header=None,low_memory=True)#filename　読み込み
                print("PATH:", filename, "READ:", basename)
               
                #print(df_BNO)
                pd.set_option('display.max_columns', 40)
                # ----bnitowの異常値修正 異常値は、前の正常値でインクリメントして埋め          
                df_BNO = df_BNO.drop(df_BNO.index[-1])  # last コメント除去
                if len(df_BNO.columns)>8: 
                    df_BNO=df_BNO.drop(range(7,len(df_BNO.columns)),axis=1 ) 
                    print(df_BNO.head())   
                    #df_BNO = df_BNO.drop(["com1", "com2", "com3"], axis=1)
                #print("SortMae:",df_BNO)
                # df_BNO=bnitowRe(df_BNO)# bnitow=0行をitowインクリメントで埋める関数
                df_BNO.columns=list_BNO0
                df_BNO = bnitowsort(df_BNO)
                #print("Prepaired df_BNO:", df_BNO)
                #print(df_BNO.dtypes)
                df_BNO.to_csv(r'C:/RTK_Log/BNOsorted_' + basename)  # CSV save
                cmnt = "[" + basename + "]" + cmnt
                print(f"df_BNO memory usage: {df_BNO.memory_usage(deep=True).sum() / 1024 ** 2} MB")

                # -----------result."iTow_B"とdf_BNO120の同期--------------
                #df_BNO = df_BNO.drop(df_BNO.index[-1])
                #print("itow120mae",df_BNO)
                df_BNO120 = itow120df(df_BNO)  # 120周期のDF作成
                df_BNO120.to_csv(r'C:/RTK_Log/BNO120ed_' + basename)  # CSV save
                print("df_BNO120 made", df_BNO120.iloc[0,0],"-",df_BNO120.iloc[- 1,0])
                print("result=",result.loc[0,"iTOW_B"],"-",result.loc[result.shape[0] - 1,"iTOW_B"])
                list_bnoit = []  # listは、数値を入れれば1次元　listを入れれば２次元になる
                bnc = []
                if ubxflag == 1:  # resultが準備されていること
                    # start行番号検索
                    itb0 = result.loc[0, "iTOW_B"]
                    itb0tail=int(result.iloc[-1,0])
                    itbn0 = int(df_BNO120.iloc[0, 0])
                    itbn0tail=int(df_BNO120.iloc[-1,0])
                    isa = int(itb0 - itbn0)
                    print("itb0=", itb0,":itb0tail=",itb0tail)
                    print("itbn0=", itbn0,":itbn0tail=",itbn0tail)
                    print("isa=", isa)
                    if itbn0>itb0tail:
                        print(" itbn0>itb0tail OUT OF RANGE")
                        exit()
                    elif itb0>itbn0tail:
                        print("  itb0>itbn0tail OUT OF RANGE")
                        exit()
                
                    

                    if isa < 10000:  # baseが先行ある
                        basestart = abs(int(isa / 120))
                        bnostart = 0
                    else:  # bnoが先行
                        basestart = 0
                        bnostart = abs(int(isa / 10))
                    print("isa=", isa, "basestart=", basestart, "bnostart=", bnostart)
                    print("basestart itow=", result.iloc[basestart, 0], "bnostart itow=", df_BNO120.iloc[0, 0])
                    targetitow = int(result.iloc[basestart, 0])
                    # result左端にdf_bno120を追加するために一旦listにして再構築
                    # 0からbasestart
                    list_bno = []#listを作成してからDFに変換
                    r = []#作業用list
                    if basestart > bnostart:  # bnostartの上にbasestart分のゼロDFを作成して合体
                        list_bno = [[0] * 7] * (basestart)  # 上のデータゼロ埋め
                        # print(list_bno)
                        blen = len(df_BNO120)
                        for i in range(0, blen - 1):  # 中間にdf_BNO120をlistに追加
                            r = df_BNO120.iloc[i, :]
                            list_bno.append(r)
                        sn = basestart + len(df_BNO120)
                        rn = len(result)
                        print("Upper-listbno=", len(list_bno))
                        print("for sn=", sn, "rn=", rn, "result len=", len(result))
                        for i in range(sn, rn + 1):  # 下のデータ埋め
                            l0 = [0, 0, 0, 0, 0, 0, 0]
                            list_bno.append(l0)
                        #print("listbno=", len(list_bno))
                        df_BNOL = pd.DataFrame(list_bno, columns=list_BNO0)
                        df_BNOL.to_csv(r'C:/RTK_Log/df_BNOL_' + basename)  # CSV save
                        # 現在の最大表示行数の出力

                        pd.set_option('display.max_rows', None)
                        pd.set_option('display.max_columns', None)
                        #print(df_BNOL)
                        print(result["iTOW_B"])
                        print("len df_BNOL=", len(df_BNOL))
                        print("len result=", len(result))
                        print("Check basestart=", basestart, "@iTOW_B=", result.iloc[220, 0], "df_BNOL bnitow=",df_BNOL.loc[220, "bnitow"])
                        # concat
                        print("Concat:df_BNOL=", df_BNOL.iloc[0,0],"-",df_BNOL.iloc[- 1,0])
                        print("Concat:result=",result.loc[0,"iTOW_B"],"-",result.loc[result.shape[0] - 1,"iTOW_B"])
                        resultBRBN = pd.concat([result, df_BNOL], axis=1)
                        additions2(resultBRBN)#skid計算,heading-yaw差計算H-y
                        count=linereg(resultBRBN,9)#直線回帰で直進部分を計算して"linecorr"に代入
                        print("linear regression count=",count)
                        #additions5(resultBRBN)#UDP用yaw補正
                        pd.set_option('display.max_rows', 50)
                        pd.set_option('display.max_columns', 100)
                        print(resultBRBN)
                        resultBRBN.to_csv(r'C:/RTK_Log/BRBN_' + basename)  # CSV save
                        brbnOK=1
                        # CPLT用ファイル
                        df_CPLT = pd.DataFrame()
                        df_CPLT = resultBRBN[["Heading360", "yaw", "headMotMA3", "diffzero", "roll"]]
                        #print("df_CPLT")
                        df_CPLT.to_csv(r'C:/RTK_Log/CPLT_BRBN_' + basename, index=False)  # CSV save
                        BRBNflag=True
                        
                    elif basestart < bnostart:  # bnostartをカットしてresultと合体
                        print("============basestart<bnostart============")
                        """
                        #bnostartがresultの何番目に位置するか検索
                        #indx2=[result["iTOW_B"]== itbn0].index
                        indx2=result[result["iTOW_B"]< itbn0]
                        print(indx2)
                        list_bno = [[0] * 7] * (basestart)  # 上のデータゼロ埋め
                        # print(list_bno)
                        blen = len(df_BNO120)
                        for i in range(0, blen - 1):  # 中間にdf_BNO120をlistに追加
                            r = df_BNO120.iloc[i, :]
                            list_bno.append(r)
                        sn = basestart + len(df_BNO120)
                        rn = len(result)
                        print("Upper-listbno=", len(list_bno))
                        print("for sn=", sn, "rn=", rn, "result len=", len(result))
                        for i in range(sn, rn + 1):  # 下のデータ埋め
                            l0 = [0, 0, 0, 0, 0, 0, 0]
                            list_bno.append(l0)
                        print("listbno=", len(list_bno))
                        df_BNOL = pd.DataFrame(list_bno, columns=list_BNO0)
                        # 現在の最大表示行数の出力

                        pd.set_option('display.max_rows', None)
                        pd.set_option('display.max_columns', None)
                        print(df_BNOL)
                        print(result["iTOW_B"])
                        print("len df_BNOL=", len(df_BNOL))
                        print("len result=", len(result))
                        print("Check basestart=", basestart, "@iTOW_B=", result.iloc[220, 0], "df_BNOL bnitow=",
                              df_BNOL.loc[220, "bnitow"])
                        # concat
                        resultBRBN = pd.concat([result, df_BNOL], axis=1)
                        additions2(resultBRBN)
                        pd.set_option('display.max_rows', 50)
                        pd.set_option('display.max_columns', 100)
                        print(resultBRBN)
                        resultBRBN.to_csv(r'C:/RTK_Log/BRBN_' + basename)  # CSV save
                        """

            # *****************************************************************************************************
            # *********************RTK File BootsOn****************************************************************
            # *****************************************************************************************************

            elif "UDP" in basename and v == "r":  # BootsOn RTK_　　ファイル
                v=""
                bnohead = list(range(74))
                print(bnohead)
                df_BTBNO = pd.read_csv(filename, names=bnohead,
                                       low_memory=True)  # B 74列　U56列といびつなｃｓｖはエラーになるので、７４列のヘッダーを与える

                df_BTBNO.columns = bnohead
                df_BTUBX = df_BTBNO[df_BTBNO.iloc[:, 0] == 'U']  # id列[:,0]が'B'ならばdf_RTKとして抽出
                df_BTUBX.fillna(0)
                df_BTUBX.reset_index(drop=True)
                #print("df_BTUBX=",df_BTUBX)
                # df_BTUBX=df_BTUBX.drop(df_BTUBX.columns[[56,73]], axis=1)
                df_BTUBX = df_BTUBX.drop(columns=range(56, 74))
                df_BTUBX.columns = head0
                df_BTUBX = df_BTUBX.add_suffix('_T', axis=1)
                print("1442df_BTUBX::", df_BTUBX)
                df_BTUBX.to_csv(r'C:/RTK_Log/BTUBX_' + basename)
                df_BTBNO = df_BTBNO[df_BTBNO.iloc[:, 0] == 'B']  # id列[:,0]が'B'ならばdf_BTBNOとして抽出
                df_BTBNO=df_BTBNO.reset_index(drop=True)
                df_BTUBX=df_BTUBX.reset_index(drop=True)
                print("df_BTBNO=",df_BTBNO)
                print("df_BTUBX=",df_BTUBX)
                print("1446df_BTBNO len=",len(df_BTBNO))
                print("1447 df_BTUBX len=",len(df_BTUBX))

                # ubxとbnoの長さ合わせ
                tsa = len(df_BTUBX) - len(df_BTBNO)
                if tsa < 0:  # ubxが短いならubxに合わせる
                    lastrow=df_BTBNO.shape[0]-1
                    print(df_BTBNO.tail(2))
                    sa=-tsa
                    #df_BTBNO = df_BTBNO.drop(range(lastrow+tsa,lastrow),axis=0)
                    df_BTBNO = df_BTBNO.iloc[:tsa]
                else:
                    #df_BTUBX = df_BTUBX.iloc[-tsa]
                    lastrow=df_BTUBX.shape[0]-1
                    df_BTUBX=df_BTUBX.drop(range(lastrow-tsa,lastrow))
                print("tsa=", tsa, "ubx_length=", len(df_BTUBX), "bno_length=", len(df_BTBNO))
                df_BTBNO.to_csv(r'C:/RTK_Log/BTBNO_' + basename)
                #BTBNOを１列に並べ替える
                df_BTBNO2=BTBNO21(df_BTBNO)
                print("df_BTBNO2:",df_BTBNO2.head(1),"\n",df_BTBNO2.tail(1))
                print("df_BTBNO=",df_BTBNO)
                print("df_BTUBX=",df_BTUBX)
                # =============Plot Vector3用にplot用dfを作成=====================
                # resultBRBNの開始targetitow
                df_BTplot = df_BTUBX[["iTOW_T", "height_T", "gSpeed_T", "headMot_T", "relPosN_T", "relPosE_T"]]
                df_BTplot = df_BTplot.reset_index(drop=True)
                # BNO列も追加
                print("df_BTBNO head=",df_BTBNO.head(1))
                byaw = list(df_BTBNO.iloc[:, 5])
                bpitch = list(df_BTBNO.iloc[:, 6])
                broll = list(df_BTBNO.iloc[:, 7])
                print("braw len=",len(byaw))
                print("bpitch len=",len(bpitch))
                print("broll len = ",len(broll))
                print("len df_BTplot col=",len(df_BTplot.columns))
                print("dfBTplot len=",len(df_BTplot))
                print("df_BTplot:",df_BTplot)
                # df_BTBNOからコピーして、df_BTplotの左端に新規列を追加してペースト　insert（）
                df_BTplot.insert(len(df_BTplot.columns), 'yaw_T', byaw)
                df_BTplot.insert(len(df_BTplot.columns), 'pitch_T', bpitch)
                df_BTplot.insert(len(df_BTplot.columns), 'roll_T', broll)
                df_BTplot = df_BTplot.reset_index(drop=True)
                print(df_BTplot)
                df_BTplot.to_csv(r'C:/RTK_Log/BTplot_' + basename)
                df_BTplot["iTOW_T"]=df_BTplot["iTOW_T"].astype('int64')
                df_BTplot["relPosN_T"] = df_BTplot["relPosN_T"].astype('float64')
                df_BTplot["headMot_T"] = df_BTplot["headMot_T"].astype('float64')
                df_BTplot["yaw_T"] = df_BTplot["yaw_T"].astype('float64')
                #yaw_Tを360座標yaw_T360に変換
                df_BTplot["yaw_T"] = np.where(df_BTplot["yaw_T"] < 0, df_BTplot["yaw_T"]+ 360, df_BTplot["yaw_T"])
                df_BTplot["pitch_T"] = df_BTplot["pitch_T"].astype('float64')
                print(df_BTplot.dtypes)
                print(df_BTplot)
                # df_BTplot600を作成
                df_BTplot600 = df_BTplot

                #bol=df_BTplot[df_BTplot["iTOW_T"]]
                #print(df_BTplot)
                #print(df_BTplot["iTOW_T"])
                drop_index = df_BTplot600.index[(df_BTplot600["iTOW_T"]) %600== 300]#600msecで300msec余る行を削除するindex
                df_BTplot600.drop(drop_index, inplace=True)#%600=300の行を削除
                df_BTplot600 = df_BTplot600.reset_index(drop=True)  # index reset
                df_BTplot600 = additions4(df_BTplot600)
                #df_BTplot600 = additions5(df_BTplot600)
                print("df_BTplot600:")
                print(df_BTplot600)
                #先頭行のitowを%600==0にあわせる
                top600_index = df_BTplot600.index[(df_BTplot600["iTOW_T"]) % 600 ==0]
                trow=top600_index[0]

                #del_index=df_BTplot600.index[0,trow]
                df_BTplot600.drop(range(0,trow), inplace=True)  # %600=0を先頭にする
                df_BTplot600 = df_BTplot600.reset_index(drop=True)  # index reset
                print(df_BTplot600)
                df_BTplot600.to_csv(r'C:/RTK_Log/BTplot600_' + basename)
                cmnt = "[" + basename + "]" + cmnt
                #=============BRBNファイルと合体してBRBNTファイル作成====================================================
                if brbnOK==1:#BRBNファイル読み込み済み
                    print("=============df_BTplot  add to resultBRBN ==============")
                    #print(resultBRBN)
                    #print(df_BTplot)
                    df_BRBNT=RTK2BRBN(resultBRBN, df_BTplot600)
                    df_BRBNT=df_BRBNT.fillna(0)
                    print(df_BRBNT)
                    df_BRBNT.to_csv(r'C:/RTK_Log/BRBNT_' + basename)
                    btOK=1

        elif v == "h" and ubxflag == 1:
            cmnt = "["+basename+ "]" + cmnt
            #plotheadmot(result, basen)
        elif v=="i" :# Integral
            
             bnointegral()

def BTBNO21(df):#BTBNOを縦１列に変換 bnitowのズレをチェックする
    #[5:10][12:17][19:24][26:31][33:38][]
    print()
    listbndata=[]
    for i in range(0,len(df)):
        bnitow=int(df.iloc[i,2])
        itowN=0
        for j in range(5,68,7):
            #print(df.iloc[i,j-1],df.iloc[i,j],df.iloc[i,j+1],df.iloc[i,j+2],df.iloc[i,j+3],df.iloc[i,j+4],df.iloc[i,j+5])
            rowdata=[]
            n=bnitow+itowN
            rowdata=[n,df.iloc[i,j],df.iloc[i,j+1],df.iloc[i,j+2],df.iloc[i,j+3],df.iloc[i,j+4],df.iloc[i,j+5]]
            itowN=itowN+10
            #print("i=",i,":",rowdata)
            listbndata.append(rowdata)
    df_rowdata=pd.DataFrame(listbndata)
    
    df_rowdata.columns=["bnitow","yaw_T","pitch_T","roll_T","ax_T","ay_T","az_T"]
    df_rowdata=df_rowdata.astype("float64")
    # yaw_T 0-360度に変換
    df_rowdata["yaw_T"] = np.where(df_rowdata["yaw_T"] < 0, df_rowdata["yaw_T"]+ 360, df_rowdata["yaw_T"])
    df_rowdata.to_csv(r'C:/RTK_Log/BTBNO21_' + basename)
    return df_rowdata
main()