# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3
#     language: python
#     name: python3
# ---

# # 0 Library & Path

# + _uuid="8ba9bcdd-cb1c-4444-a205-00bc3cbdddc9" _cell_guid="02ebe3d8-97df-4862-adba-ebc1ab0c5036" _kg_hide-input=false _kg_hide-output=true
import numpy as np 
import pandas as pd 


import json
from pathlib import Path

import os
import matplotlib.pyplot as plt

# + [markdown] _uuid="1a06b7af-a8fa-4384-a972-2871f3409144" _cell_guid="f2e582d1-eeee-45c7-b69e-e9fddcb393eb"
# Clone the github repro provided by the competition since there are some useful functions

# + _uuid="ec26b9ec-d8ab-4d64-a989-6c26260e2e1f" _cell_guid="395c2d3c-a612-4bc6-9421-9d730f8bfd3a"
# !git clone https://github.com/location-competition/indoor-location-competition-20.git api
# -

from api.compute_f import *
from api.io_f import * 
from api.visualize_f import *

# [AHRS](https://pypi.org/project/AHRS/) is a collection of functions and algorithms in pure Python used to estimate the orientation of mobile systems.
#
# Orginally, an AHRS is a set of orthogonal sensors providing attitude information about an aircraft. This field has now expanded to smaller devices, like wearables, automated transportation and all kinds of systems in motion.
#
# It also provide functions like EKF (Extended Kalman Filter).

# !pip install AHRS

from ahrs.filters import EKF
from ahrs.common.orientation import acc2q
from ahrs.utils import WMM
from scipy.spatial.transform import Rotation as R

# + _uuid="d54f12f7-8fe7-4af1-861b-2c205d595bd9" _cell_guid="df31f87a-1bd0-41fd-ae07-2b6e687beafa"
# have a look at accel data
sample_txt_file_path = Path('../input/indoor-location-navigation/train/5a0546857ecc773753327266/B1/5e15730aa280850006f3d005.txt')
# sample_txt_file_path
train_floor_data = read_data_file(sample_txt_file_path)
acce_df = pd.DataFrame(train_floor_data.acce)
acce_df.columns = ['timestamp', 'acce_x','acce_y','acce_z']
print(acce_df.shape)
acce_df.head(10)
# -

# have a look at waypoints data
waypoints_df = pd.DataFrame(train_floor_data.waypoint)
waypoints_df.columns = ['timestamp', 'waypoint_x','waypoint_y']
print(waypoints_df.shape)
waypoints_df.head(10)

#read file for extra grid points
extra_grid_points = pd.read_csv('../input/note-2-generate-extra-grid-points/extra_grid_points.csv')
extra_grid_points.head(10)

# +
test_extra_grid = extra_grid_points[['x','y']]

test_extra_grid.shape
# -

frames = [test_extra_grid,test_extra_grid]
result_test = pd.concat(frames)
result_test.shape

# # 1 Functions

# ## 1.1 support functions.

# +
"""
This function query all waypoints for a given building and floor:
Input:
0-dataframe with all waypoints.
1-site_No
2-floor_No
Output:
1-waypoints_buid_flr
"""
def query_waypoints(waypoints_all, extra_grid_points,site_in,floor_in):
    
    waypoints_buid_flr = waypoints_all.query('site == @site_in and floorNo == @floor_in').copy()
    gridpoints_build_flr = extra_grid_points.query('site == @site_in and floor == @floor_in').copy()
    
    waypoints_buid_flr_xy = waypoints_buid_flr[['x','y']]
    gridpoints_build_flr_xy = gridpoints_build_flr[['x','y']]
    
    waypoints_concat = [waypoints_buid_flr_xy,gridpoints_build_flr_xy]
        
    return pd.concat(waypoints_concat)

"""
This function find one closest point
"""
def find_closest_point(waypoints,point_in):
    
    points = waypoints[["x","y"]].to_numpy()
    dist_2 = np.sum((points - point_in)**2, axis=1)
    point_arry = np.argmin(dist_2)
    
    distance = dist_2[point_arry]
    point_closest = points[point_arry,:]
    return point_closest, distance

"""
This function find the first n closest point
input: num:number of closest points

"""
def find_closest_point_multiple(waypoints,point_in,num):
    
    points = waypoints[["x","y"]].to_numpy()
    dist_2 = np.sum((points - point_in)**2, axis=1)
    point_ind = np.argpartition(dist_2, num)[:num]
    
    distance_n = dist_2[point_ind]
    point_closest_n = points[point_ind,:]
    return point_closest_n, distance_n


# -

# ## 1.2 Position calculation functions

# +
"""
This function calculate the tajectory using IMU for one step:
Input:
1-accel_select, ahrs_select:contains IMU information needed for one step (output from function get_section_traject)
2-waypoints_buid_flr: the way points for that building and floor.
3-start_point: for this function it is an input.
Output:
1-predicted_positions
2-total_error: each step, there is an error. Sum them up.
"""

def predict_pos_one_step(accel_select, ahrs_select,start_point_x, start_point_y): 
    
    # Compute relative position
    step_timestamps, step_indexs, step_acce_max_mins = compute_steps(accel_select)
    headings = compute_headings(ahrs_select)
    stride_lengths = compute_stride_length(step_acce_max_mins)
    step_headings = compute_step_heading(step_timestamps, headings)
    rel_positions = compute_rel_positions(stride_lengths, step_headings)
    
    rel_positions[:, 1].cumsum(), rel_positions[:, 2].cumsum()
    
    predicted_positions = np.zeros(rel_positions.shape)
    predicted_positions[:, 0] = rel_positions[:, 0]
    predicted_positions[:, 1] = rel_positions[:, 1].cumsum() + start_point_x
    predicted_positions[:, 2] = rel_positions[:, 2].cumsum() + start_point_y
    
    return predicted_positions

"""
This function snap to grid:

"""
def snap_grid(waypoints,start_point_x, start_point_y):
    
    point_in = (start_point_x,start_point_y)
    point_closest, distance = find_closest_point(waypoints,point_in)
    
    return point_closest, distance


"""
This function get a section of the tajectory  between time t_start and t_end:
Input:
1-trajectory_file:contains IMU information needed for one step

Output:
1-predicted_positions

"""
def get_section_traject(trajectory_file,t_start,t_end):
    sample_data = read_data_file(str(trajectory_file))
    
    accel_ekf = apply_ekf(sample_data)

    accel_narry_original = sample_data.acce  
    ahrs_narry = sample_data.ahrs
    
    time_stamp = np.reshape(ahrs_narry[:,0],(-1,1))
    accel_narry = np.concatenate((time_stamp, accel_ekf), axis=1)
    
    accel_select = accel_narry[(accel_narry[:,0]>t_start) & (accel_narry[:,0]<t_end) ]
    ahrs_select = ahrs_narry[(ahrs_narry[:,0]>t_start) & (ahrs_narry[:,0]<t_end) ]

    return accel_select, ahrs_select

"""
This function apply extended kalman filter (EKF):
Input:
1-sample_data

Output:
1-new_acce: acceleration after EKF

"""
def apply_ekf(sample_data):
    ekf_estimator = EKF(gyr=sample_data.gyro[:,1:4], acc=sample_data.acce[:,1:4], mag=sample_data.magn[:,1:4], frequency=50.0)
    # Initializing the class with the sensor data generates our Quaternion in the Q Variable

    Q = ekf_estimator.Q
    n_samples = sample_data.acce.shape[0]
    new_acce = np.zeros((n_samples, 3))
    # Initializing Array to hold the Linear acceleration Vector

    for t in range(n_samples):
        r = R.from_quat(Q[t])
        # Getting a Rotation Matrix from the Quaternions
        new_acce[t] = np.matmul(r.as_matrix().T,sample_data.acce[t][1:4])
        # Multiply Rotation Matrix Transpose to orignal Acceleration to produce the new acceleration
    #print(new_acce)
    return new_acce


# -

# ## 2 tests for function development

# +
"""
example trajectary
"""

example_site = '5a0546857ecc773753327266'
example_floorNo = 'B1'
example_trajectorty = '5e1573121506f2000638fc2f.txt'

example_path = '../input/indoor-location-navigation/train/'+example_site+'/'+example_floorNo+'/'+example_trajectorty
print(example_path)
# -

"""
read grid points
"""
train_waypoints = pd.read_csv('../input/indoor-location-train-waypoints/train_waypoints.csv')

# +
"""
Test code - for one point
"""
# step 1 - read data - start position
trajectory_file_path = Path(example_path)
train_floor_data = read_data_file(trajectory_file_path)

# get t_start and t_end
#waypoints_df = pd.DataFrame(train_floor_data.waypoint)
#waypoints_df.columns = ['timestamp', 'waypoint_x','waypoint_y']

waypoints_np = train_floor_data.waypoint
t_start = waypoints_np[0,0]
t_end = waypoints_np[1,0]
print('start and end time')
print(t_start,t_end)

# get start_point_x, start_point_y
start_point_x = waypoints_np[0,1]
start_point_y = waypoints_np[0,2]
print('start point')
print(start_point_x,start_point_y)

# get waypoints for the floor of the building
waypoints_buid_flr = query_waypoints(train_waypoints, extra_grid_points,example_site,example_floorNo)

# step 2 - calculate position - IMU
accel_select, ahrs_select = get_section_traject(trajectory_file_path,t_start,t_end)
print(accel_select)
print(ahrs_select)
predicted_positions = predict_pos_one_step(accel_select, ahrs_select,start_point_x, start_point_y)
pred_x = predicted_positions[-1,1]
pred_y = predicted_positions[-1,2]
print('IMU predict point')
print(predicted_positions)
print(pred_x,pred_y)

# step 3 - calculate position - snap to point
point_closest, distance = snap_grid(waypoints_buid_flr,pred_x, pred_y)
print('snap predict point')
print(point_closest)
print(distance)

# step 4 - evaluation error
# print ground truth
print(waypoints_np[1,1],waypoints_np[1,2])

# +
"""
Test code - for a trajectory - training data - search multiple starting point
"""

# step 1 - read data - start position
trajectory_file_path = Path(example_path)
train_floor_data = read_data_file(trajectory_file_path)

# get t_start and t_end
#waypoints_df = pd.DataFrame(train_floor_data.waypoint)
#waypoints_df.columns = ['timestamp', 'waypoint_x','waypoint_y']

waypoints_np = train_floor_data.waypoint
t_start = waypoints_np[0,0]
t_end = waypoints_np[1,0]
print('start and end time')
print(t_start,t_end)

# get waypoints for the floor of the building
waypoints_buid_flr = query_waypoints(train_waypoints, extra_grid_points,example_site,example_floorNo)

# get start_point_x, start_point_y
start_point_x_for_search = waypoints_np[0,1]
start_point_y_for_search = waypoints_np[0,2]
point_in = (start_point_x_for_search,start_point_y_for_search)
num = 10 # how many points to search
point_closest_n, distance_n = find_closest_point_multiple(waypoints_buid_flr,point_in,num)

print('start point')
print(point_closest_n)


for row in point_closest_n:
    start_point_x = row[0]
    start_point_y = row[1]
    print('search point')
    print(row)
    
    # step 2 - calculate position - IMU
    accel_select, ahrs_select = get_section_traject(trajectory_file_path,t_start,t_end)
    predicted_positions = predict_pos_one_step(accel_select, ahrs_select,start_point_x, start_point_y)
    pred_x = predicted_positions[-1,1]
    pred_y = predicted_positions[-1,2]
    #print('IMU predict point')
    #print(predicted_positions)
    #print(pred_x,pred_y)
    
    # step 3 - calculate position - snap to point
    point_closest, distance = snap_grid(waypoints_buid_flr,pred_x, pred_y)
    print('snap predict point')
    print(point_closest)
    print(distance)
    
# -

# # 3 Position calculation

# read baseline file
baseline_df = pd.read_csv("../input/baseline/submission_baseline.csv",index_col=False)
# divide dataframe into site path timestamp
baseline_df[['site','path','timestamp']] = baseline_df['site_path_timestamp'].str.split('_',expand=True)
baseline_df.head(10)

baseline_df_groupby = baseline_df.groupby(['site','path'])
baseline_df_groupby.head(5)

# +
floor_map = {"B2":-2, "B1":-1, "F1":0, "F2": 1, "F3":2,
             "F4":3, "F5":4, "F6":5, "F7":6,"F8":7,"F9":8,
             "1F":0, "2F":1, "3F":2, "4F":3, "5F":4, "6F":5,
             "7F":6, "8F": 7, "9F":8}

# list out keys and values separately
key_list = list(floor_map.keys())
val_list = list(floor_map.values())
 
# print key with val 100
position = val_list.index(-1)
print(key_list[position])

# +
"""
Searching the starting point from the submission baseline
"""
import time

# hyper parameter
num = 5 # how many start points to search
dis_thresold = 4 # if to replace the baseline x, y

submission_df = pd.DataFrame() # define an empty frame
sub_group_df = pd.DataFrame()

start_time = time.time()
cnt_group=0
for group_name, df_group in baseline_df_groupby: # loop for the submission baseline file
    example_site = df_group.iloc[0]['site']
    example_floorValue = df_group.iloc[0]['floor']
    position = val_list.index(example_floorValue)
    example_floorNo = key_list[position]
    example_trajectorty = df_group.iloc[0]['path']
    time_stamp = df_group['timestamp']
    
    # step 0 - read files
    example_path = '../input/indoor-location-navigation/test/'+example_trajectorty+'.txt'
    trajectory_file_path = Path(example_path)
    train_floor_data = read_data_file(trajectory_file_path)
    
    # get waypoints for the floor of the building
    waypoints_buid_flr = query_waypoints(train_waypoints,extra_grid_points,example_site,example_floorNo)
    
    # step 1 - get start position 'num' number of start position
    start_point_x_for_search = df_group.iloc[0]['x']
    start_point_y_for_search = df_group.iloc[0]['y']
    point_in = (start_point_x_for_search,start_point_y_for_search)
    point_closest_n, distance_n = find_closest_point_multiple(waypoints_buid_flr,point_in,num)
    
    last_dist = 1e16
    for row_pt in point_closest_n: # for different starting point
        sum_dis = 0 # reset distance for each start point
        cnt_stamp = 0
        arr_x_y = []
        
        start_point_x = row_pt[0]
        start_point_y = row_pt[1]
        
        arr_x_y.append([start_point_x,start_point_y])
        
        last_row_time=0
        for row_time in time_stamp:
            if cnt_stamp!=0: #skip the first
                t_start = int(last_row_time)
                t_end = int(row_time)

                accel_select, ahrs_select = get_section_traject(trajectory_file_path,t_start,t_end)
                predicted_positions = predict_pos_one_step(accel_select, ahrs_select,start_point_x, start_point_y)
                pred_x = predicted_positions[-1,1]
                pred_y = predicted_positions[-1,2]
                
                # snap to point
                point_closest, distance = snap_grid(waypoints_buid_flr,pred_x, pred_y)
                
                # update start point to next position
                start_point_x = point_closest[0]
                start_point_y = point_closest[1]
                
                arr_x_y.append([start_point_x,start_point_y])
                                
                sum_dis = sum_dis+distance
                
            cnt_stamp = cnt_stamp+1
            last_row_time = row_time
        avg_dis = sum_dis/cnt_stamp
            
        if (avg_dis<dis_thresold) & (avg_dis<last_dist):
            df_group[['x','y']]=arr_x_y
            #print('find one! replace!')
        last_dist = avg_dis
  
    submission_df = submission_df.append(df_group)
    
    cnt_group = cnt_group+1
    
    if cnt_group%10 == 0:
        #timer
        current_time = time.time()
        elapsed_time = current_time - start_time
        print(elapsed_time)
        start_time = time.time()
    
submission_df.sort_index(inplace=True)    
print(submission_df.shape)
submission_df.head(10)
# -

# # 4 Output submission file

# output file
submission_pd_to_file = submission_df.drop(['site','path','timestamp'],axis=1)
submission_pd_to_file.head(10)
submission_pd_to_file.to_csv('submission_file.csv', index=False)

StarcoderdataJupyterScriptsDedupFiltered