# -*- coding:utf-8 -*-

import os

import cv2

import pandas as pd

import numpy as np

import configparser as cp

import matplotlib.pyplot as plt

RAW_DATA_PATH = '/home/tony/fall_research/fall_data/MobiAct_Dataset_v2.0/Annotated Data/'

Label = {'STD': 1, 'WAL': 2, 'JOG': 3, 'JUM': 4, 'STU': 5, 'STN': 6, 'SCH': 7, 'SIT': 8, 'CHU': 9,

         'LYI': 10, 'FOL': 0, 'FKL': 0, 'BSC': 0, 'SDL': 0, 'CSI': 15, 'CSO': 16}

def extract_data(data_file, sampling_frequency):

    """

    从mobileFall中提取数据，用于做实验测试

    :param data_file:  原始数据文件

    :param sampling_frequency: 原始数据采集频率

    :return:

    """

    data = pd.read_csv(data_file, index_col=0)

    data_size = len(data.label)

    for i in range(data_size):

        data.iat[i, 10] = Label[data.iloc[i, 10]]

    col_data = np.arange(0, data_size, int(sampling_frequency/50))

    extract_data = data.iloc[col_data, [1, 2, 3, 4, 5, 6, 10]]

    save_path = './dataset/raw/' + os.path.abspath(os.path.dirname(data_file)+os.path.sep+".").replace(RAW_DATA_PATH, '')

    if not os.path.exists(save_path):

        os.makedirs(save_path)

    save_path = './dataset/raw/' + data_file.replace(RAW_DATA_PATH, '')

    extract_data.to_csv(save_path, index=0)

def find_all_data_and_extract(path):

    """

    递归的查找所有文件并进行转化

    :param path:

    :return:

    """

    if not os.path.exists(path):

        print('路径存在问题：', path)

        return None

    for i in os.listdir(path):

        if os.path.isfile(path+"/"+i):

            if 'csv' in i:

                extract_data(path+"/"+i, 200)

        else:

            find_all_data_and_extract(path+"/"+i)

def parser_cfg_file(cfg_file):

    """

    读取配置文件中的信息

    :param cfg_file: 文件路径

    :return:

    """

    content_params = {}

    config = cp.ConfigParser()

    config.read(cfg_file)

    for section in config.sections():

        # 获取配置文件中的net信息

        if section == 'net':

            for option in config.options(section):

                content_params[option] = config.get(section,option)

        # 获取配置文件中的train信息

        if section == 'train':

            for option in config.options(section):

                content_params[option] = config.get(section,option)

    return content_params

def show_data(data, name=None):

    '''

    show data

    :param data: DataFrame

    :return:

    '''

    num = data.acc_x.size

    x = np.arange(num)

    fig = plt.figure(1, figsize=(100, 60))

    # 子表1绘制加速度传感器数据

    plt.subplot(2, 1, 1)

    plt.title('acc')

    plt.plot(x, data.acc_x, label='x')

    plt.plot(x, data.acc_y, label='y')

    plt.plot(x, data.acc_z, label='z')

    # 添加解释图标

    plt.legend()

    x_flag = np.arange(0, num, num / 10)

    plt.xticks(x_flag)

    # 子表2绘制陀螺仪传感器数据

    plt.subplot(2, 1, 2)

    plt.title('gyro')

    plt.plot(x, data.gyro_x, label='x')

    plt.plot(x, data.gyro_y, label='y')

    plt.plot(x, data.gyro_z, label='z')

    plt.legend()

    plt.xticks(x_flag)

    #plt.show()

    if name is None:

        plt.show()

    else:

        plt.savefig(name)

    plt.close()

def kalman_filter(data):

    kalman = cv2.KalmanFilter(6, 6)

    kalman.measurementMatrix = np.array([[1, 0, 0, 0, 0, 0],

                                         [0, 1, 0, 0, 0, 0],

                                         [0, 0, 1, 0, 0, 0],

                                         [0, 0, 0, 1, 0, 0],

                                         [0, 0, 0, 0, 1, 0],

                                         [0, 0, 0, 0, 0, 1]], np.float32)

    kalman.transitionMatrix = np.array([[1, 0, 0, 0, 0, 0],

                                         [0, 1, 0, 0, 0, 0],

                                         [0, 0, 1, 0, 0, 0],

                                         [0, 0, 0, 1, 0, 0],

                                         [0, 0, 0, 0, 1, 0],

                                         [0, 0, 0, 0, 0, 1]], np.float32)

    kalman.processNoiseCov = np.array([[1, 0, 0, 0, 0, 0],

                                       [0, 1, 0, 0, 0, 0],

                                       [0, 0, 1, 0, 0, 0],

                                       [0, 0, 0, 1, 0, 0],

                                       [0, 0, 0, 0, 1, 0],

                                       [0, 0, 0, 0, 0, 1]], np.float32) * 0.003

    kalman.measurementNoiseCov = np.array([[1, 0, 0, 0, 0, 0],

                                          [0, 1, 0, 0, 0, 0],

                                          [0, 0, 1, 0, 0, 0],

                                          [0, 0, 0, 1, 0, 0],

                                          [0, 0, 0, 0, 1, 0],

                                          [0, 0, 0, 0, 0, 1]], np.float32) * 1

    row_num = data.acc_x.size

    for i in range(row_num):

        correct = np.array(data.iloc[i, 0:6].values, np.float32).reshape([6, 1])

        kalman.correct(correct)

        predict = kalman.predict()

        data.iloc[i, 0] = predict[0]

        data.iloc[i, 1] = predict[1]

        data.iloc[i, 2] = predict[2]

        data.iloc[i, 3] = predict[3]

        data.iloc[i, 4] = predict[4]

        data.iloc[i, 5] = predict[5]

    return data

def find_all_data_and_filtrate(path):

    """

    递归的查找所有文件并进行kalman过滤

    :param path:

    :return:

    """

    if not os.path.exists(path):

        print('路径存在问题：', path)

        return None

    for i in os.listdir(path):

        if os.path.isfile(path+"/"+i):

            if 'csv' in i:

                data = pd.read_csv(path+"/"+i)

                data = kalman_filter(data)

                data.to_csv(path+"/"+i, index=False)

        else:

            find_all_data_and_filtrate(path+"/"+i)

def main():

    #find_all_data_and_extract(RAW_DATA_PATH)

    find_all_data_and_filtrate('./dataset/kalman/')

if __name__ == '__main__':

    main()

    # if os.path.exists('./dataset/train/BSC_1_1_annotated.csv') == False:

    #     print('./dataset/train/BSC_1_1_annotated.csv', '文件不存在！')

    # data = pd.read_csv('./dataset/train/BSC_1_1_annotated.csv')

    #

    # #show_data(data)

    # data = kalman_filter(data)

    # data.to_csv('./dataset/train/BSC_1_1_annotated.csv', index=False)

    # #show_data(data)

    # # a = data.iloc[4:5,0]

    # # print(a)

    # data = pd.read_csv('./dataset/train/STU_1_1_annotated.csv')

    #

    # show_data(data)