%     NeuroSLAM System Copyright (C) 2018-2019 
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com) 
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013). 
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
% 
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
% 
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
% 
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
% 
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

clear workspace
% global expTrajectory;
% vo initial
visual_odo_initial( ...
    'ODO_IMG_TRANS_Y_RANGE', 31:90, ...
    'ODO_IMG_TRANS_X_RANGE', 16:145, ...
    'ODO_IMG_HEIGHT_V_Y_RANGE', 11:110, ...
    'ODO_IMG_HEIGHT_V_X_RANGE', 11:150, ...
    'ODO_IMG_YAW_ROT_Y_RANGE', 31:90, ...
    'ODO_IMG_YAW_ROT_X_RANGE', 16:145, ...
    'ODO_IMG_TRANS_RESIZE_RANGE', [60, 130], ...
    'ODO_IMG_YAW_ROT_RESIZE_RANGE', [60, 130], ...
    'ODO_IMG_HEIGHT_V_RESIZE_RANGE', [100, 140], ...
    'ODO_TRANS_V_SCALE', 30, ...
    'ODO_YAW_ROT_V_SCALE', 1, ...
    'ODO_HEIGHT_V_SCALE', 20, ...
    'MAX_TRANS_V_THRESHOLD', 0.5, ...
    'MAX_YAW_ROT_V_THRESHOLD', 2.5, ...
    'MAX_HEIGHT_V_THRESHOLD', 0.45, ... 
    'ODO_SHIFT_MATCH_HORI', 26, ...
    'ODO_SHIFT_MATCH_VERT', 20, ...
    'FOV_HORI_DEGREE', 81.5, ...
    'FOV_VERT_DEGREE', 50, ...
    'KEY_POINT_SET', [2352, 2450, 4002, 4100], ...
    'ODO_STEP', 1);


% vt initial               
vt_image_initial('*.png', ...
    'VT_MATCH_THRESHOLD', 0.15, ...
    'VT_IMG_CROP_Y_RANGE', 1:120, ...
    'VT_IMG_CROP_X_RANGE', 1:160, ...
    'VT_IMG_RESIZE_X_RANGE', 16, ...
    'VT_IMG_RESIZE_Y_RANGE', 12, ...
    'VT_IMG_X_SHIFT', 5, ...
    'VT_IMG_Y_SHIFT', 3, ...
    'VT_GLOBAL_DECAY', 0.1, ...
    'VT_ACTIVE_DECAY', 2.0, ...
    'PATCH_SIZE_Y_K', 5, ...
    'PATCH_SIZE_X_K', 5, ...
    'VT_PANORAMIC', 0, ...
    'VT_STEP', 1);


% yaw_pitch_hdc initial 
yaw_height_hdc_initial( ...
    'YAW_HEIGHT_HDC_Y_DIM', 36, ...
    'YAW_HEIGHT_HDC_H_DIM', 36, ...
    'YAW_HEIGHT_HDC_EXCIT_Y_DIM', 8, ...
    'YAW_HEIGHT_HDC_EXCIT_H_DIM', 8, ...
    'YAW_HEIGHT_HDC_INHIB_Y_DIM', 5, ...
    'YAW_HEIGHT_HDC_INHIB_H_DIM', 5, ...
    'YAW_HEIGHT_HDC_EXCIT_Y_VAR', 1.9, ...
    'YAW_HEIGHT_HDC_EXCIT_H_VAR', 1.9, ...
    'YAW_HEIGHT_HDC_INHIB_Y_VAR', 3.0, ...
    'YAW_HEIGHT_HDC_INHIB_H_VAR', 3.0, ...
    'YAW_HEIGHT_HDC_GLOBAL_INHIB', 0.0002, ...
    'YAW_HEIGHT_HDC_VT_INJECT_ENERGY', 0.001, ...
    'YAW_ROT_V_SCALE', 1, ...  
    'HEIGHT_V_SCALE', 1, ...   
    'YAW_HEIGHT_HDC_PACKET_SIZE', 5);

% 3d gc initial
gc_initial( ...
    'GC_X_DIM', 36, ...
    'GC_Y_DIM', 36, ...
    'GC_Z_DIM', 36, ...
    'GC_EXCIT_X_DIM', 7, ...
    'GC_EXCIT_Y_DIM', 7, ...
    'GC_EXCIT_Z_DIM', 7, ...
    'GC_INHIB_X_DIM', 5, ...
    'GC_INHIB_Y_DIM', 5, ...
    'GC_INHIB_Z_DIM', 5, ...
    'GC_EXCIT_X_VAR', 1.5, ...
    'GC_EXCIT_Y_VAR', 1.5, ...
    'GC_EXCIT_Z_VAR', 1.5, ...
    'GC_INHIB_X_VAR', 2, ...
    'GC_INHIB_Y_VAR', 2, ...
    'GC_INHIB_Z_VAR', 2, ...
    'GC_GLOBAL_INHIB', 0.0002, ...  
    'GC_VT_INJECT_ENERGY', 0.1, ...   
    'GC_HORI_TRANS_V_SCALE', 1, ...
    'GC_VERT_TRANS_V_SCALE', 1, ...  
    'GC_PACKET_SIZE', 4);
   
% 3d em initial 1/2/5/
exp_initial( ...
    'DELTA_EXP_GC_HDC_THRESHOLD', 40, ...
    'EXP_LOOPS', 1, ...
    'EXP_CORRECTION', 0.5 ); 

% main process 
% file path: visualDataFile, expMapHistoryFile, odoMapHistoryFile, groundTruthFile,vtHistoryFile, emHistoryFile, varargin
imageFolderPath = 'C:\NeuroSLAM_Datasets\01_NeuroSLAM_Datasets\01_SynPerData';
SynPerDataFile = fullfile (imageFolderPath, '');

gtDataFolderPath = 'C:\NeuroSLAM_Datasets\02_NeuroSLAM_Groudtruth\';
gtDataFile = fullfile(gtDataFolderPath, '01_SynPerData_GT.txt');

experiDataPath = 'C:NeuroSLAM_Datasets\03_NeuroSLAM_Experiments_Results\SynPerData';
expMapFile  = fullfile(experiDataPath, '01_exp_map_ml.txt');
odoMapFile  = fullfile(experiDataPath, '02_odo_map_ml.txt');
emHisFile   = fullfile(experiDataPath, '03_em_history_ml.txt');
vtHisFile   = fullfile(experiDataPath, '04_vt_history_ml.txt');
gcTrajFile  = fullfile(experiDataPath, '05_gc_trajectory.txt');
hdcTrajFile = fullfile(experiDataPath, '06_hdc_trajectory.txt');

main(SynPerDataFile, ...
    gtDataFile, ...
    expMapFile, ...
    odoMapFile, ...
    emHisFile, ...
    vtHisFile, ...    
    gcTrajFile, ...
    hdcTrajFile, ...
    'BLOCK_READ', 50, ...
    'RENDER_RATE', 2, ...
    'GT_ODO_X_SCALING', 20, ...
    'GT_ODO_Y_SCALING', 20, ...
    'GT_ODO_Z_SCALING', 28, ...
    'GT_EXP_X_SCALING', 20, ...
    'GT_EXP_Y_SCALING', 20, ...
    'GT_EXP_Z_SCALING', 28, ...
    'ODO_MAP_X_SCALING', -0.08, ...
    'ODO_MAP_Y_SCALING', -0.08, ...
    'ODO_MAP_Z_SCALING', 0.1, ...
    'EXP_MAP_X_SCALING', 0.13, ...
    'EXP_MAP_Y_SCALING', -0.08, ...
    'EXP_MAP_Z_SCALING', 0.1);