% Create Harp Data Frame to trigger Laser on the Multi Pwm device
TxHarpDataFrame = CreateHarpDataFrame(60, 4, 'U8', 'Write');
TriggerLaser = HarpDataFrameToOsc(TxHarpDataFrame);

% Create Harp Data Frame to start acquisition on the Wear device
TxHarpDataFrame = CreateHarpDataFrame(32, 1, 'U8', 'Write');
StartAcquisition = HarpDataFrameToOsc(TxHarpDataFrame);

% Create Harp Data Frame to stop acquisition on the Wear device
TxHarpDataFrame = CreateHarpDataFrame(32, 0, 'U8', 'Write');
StopAcquisition = HarpDataFrameToOsc(TxHarpDataFrame);

% Create Harp Data Frame to read REG_CONF_SAMPLE_FREQ
TxHarpDataFrame = CreateHarpDataFrame(58, 0, 'U8', 'Read');
ReadSampleFrequency = HarpDataFrameToOsc(TxHarpDataFrame);

% Create Harp Data Frame to read CONF_RANGE_ACC
TxHarpDataFrame = CreateHarpDataFrame(59, 0, 'U8', 'Read');
ReadRangeAcc = HarpDataFrameToOsc(TxHarpDataFrame);

% Create Harp Data Frame to read CONF_RANGE_GYR
TxHarpDataFrame = CreateHarpDataFrame(60, 0, 'U8', 'Read');
ReadRangeGyro = HarpDataFrameToOsc(TxHarpDataFrame);

% Define RX nad TX parameters
rxPort = 51000;
txPort = 51001;
rxTimeout = 10000;  % ms
rxMaxPacketLength = 100;
txRemoteHost = '127.0.0.1';


% Open receive OSC datagram socket and create receive buffer
[WearRxSocket, rxPacket] = OpenRxDatagramSocket(rxPort, rxTimeout, rxMaxPacketLength);

% Open OSC datagram socket to send commands to the Multi Pwm device
MultiPwmTxPort = 51001;
MultiPwmTxSocket = OpenTxDatagramSocket;

% Open OSC datagram socket to send commands to the Wear device
WearTxPort = 51002;
WearTxSocket = OpenTxDatagramSocket;


%% Set OnlineProcessMotionDataJT variables
% Prompt experiment name
expt = [];
prompt = 'Please enter expt name as a string';
expt = input(prompt)

% Prompt first target cluster
targetcluster=[];
prompt = 'Please enter first targetcluster';
targetcluster = input(prompt)

%Prompt second target cluster % UDPATE
secondtargetcluster = [];
prompt = 'Please enter second targetcluster';
secondtargetcluster = input(prompt)

if exist('Library') == 0
    prompt = 'Please load desired Library of animal to workspace, press 1 to continue';
    librarycriteria = input(prompt)
end


% filter param already calculated
ProcessInput.a = [1.000000000000000  -0.984414127416097];
ProcessInput.b = [0.992207063708048  -0.992207063708048];
fs = 200;
%f_cut = 0.5;
% [b,a] = butter (1, f_cut/(fs/2), 'high'):

% -- labels = {'z GA', 'x', 'gyr x', 'log(TotAccelBA)'};
% Histogram cutoffs for the features
ProcessInput.histCutOffthresh =  {[-inf linspace(-1,1,10) inf],[-inf linspace(-1.5,1,10) inf],[-Inf,-100,-50,0,50,100,Inf],[-Inf,-3,Inf]};%-3 prev
%

% cluster processing

for i=1:length(targetcluster(1,:)) % first action
    tempfile=[];
    tempfile=cell2mat(Library.clusters(1,targetcluster(1,i)));
    ProcessInput.thresh30top(1,i)=prctile(tempfile(:,30),100);
    ProcessInput.thresh30bot(1,i)=prctile(tempfile(:,30),0);
end

for i=1:length(secondtargetcluster(1,:)) % second action% UDPATE
    tempfile=[];
    tempfile=cell2mat(Library.clusters(1,secondtargetcluster(1,i)));
    ProcessInput.secondthresh30top(1,i)=prctile(tempfile(:,30),100);
    ProcessInput.secondthresh30bot(1,i)=prctile(tempfile(:,30),0);
end


% Time variables

numcycles=[];
prompt = 'Please enter number of cycles to run experiment on';
numcycles = input(prompt)

fs=200;

framewinsize=60;%winsize in frame numbers

baselinetime1=30; %minutes to record

numbaselinecycles1=baselinetime1*60/(framewinsize/fs);

stimulationtime1=60;%stimulation minutes to record

numstimulationcycles1=stimulationtime1*60/(framewinsize/fs);

% variables

ProcessInput.dataptperframe=60; % update
ProcessInput.accRg = 4;
ProcessInput.gyrRg = 1000;
w=1;
outHist=[];

outHistset=[];
outHistset=zeros(numstimulationcycles1*numcycles*4,33); %% UPDATE 33 columns
oHcount=1;

%     secondoutHistset=[]; % UDPATE
%     secondoutHistset=zeros(numstimulationcycles1*numcycles*4,32);% UDPATE
%     soHcount=1;% UDPATE

runset=[];
runset=zeros(numstimulationcycles1*numcycles*4*framewinsize,10);
rscount=1;

timeset=[];
timeset=zeros(numstimulationcycles1*numcycles*4*framewinsize,1);
tscount=1;

elapsed=[];
elapsed=zeros(numstimulationcycles1*numcycles*4/2,1);
epcount=1;

elapsed2=[]; %UPDATE
elapsed2=zeros(numstimulationcycles1*numcycles*4/2,1);%UPDATE
epcount=1;%UPDATE REDUNDANT???

state=[];


%% Calibration 1 - acc tilt

%load stationarysensor file from calibration

if exist('stationarysensor') == 0
    prompt = 'Please load desired stationarysensor file of animal to workspace, press 1 to continue';
    stationarysensorcriteria = input(prompt)
end

% Rotate the sensor axis to a universal reference
% calibrate sensor based on stationary measure on vertical implant holder
rotMatrix = [];

x = stationarysensor.accxyz(:,3);  %accZ is x for code
z = stationarysensor.accxyz(:,1);  %accX is Z for code
y = stationarysensor.accxyz(:,2);

freq = 200;

startCalib = 1;

[xCalib, yCalib, zCalib rotMatrix] = sensorCalibrationJT(startCalib, x,y,z,freq) %plot rgb orig vs adjusted mck

%store variables in Calibrate struct
Calibrate = [];
Calibrate = struct;
Calibrate.xCalib = xCalib;
Calibrate.yCalib = yCalib;
Calibrate.zCalib = zCalib;
Calibrate.rotMatrix = rotMatrix;

%% Calibration 2 - gyroscope baseline

% Prompt Sensor ID

prompt = 'Please identify sensor as either Acc5id3 or Acc1id4';
sensorid = input(prompt)


if exist('gyrobaseline') == 0
    prompt = 'Please load desired gyrobaseline struct file of animal to workspace, press 1 to continue';
end

gyrobaseline = gyrobaselinestruct.gyrobaseline;


%% Send OSC message to start acquisition
SendTxDatagram(WearTxSocket, txRemoteHost, WearTxPort, StartAcquisition);

%% Looping through cycles of baseline-stimulation rounds

for k=1:numcycles
    %% Control baseline recording 1
    
    state=0; %indicate baseline condition
    
    for i = 0:(numbaselinecycles1-1)
        
        framesReceived = 0;
        
        % Create/clear array of data
        motionData = [];
        timeData=[];
        
        while framesReceived < 60
            % Wait for packet
            [OscRxMessage, sourceHost] = ReceiveRxDatagram(WearRxSocket, rxPacket);
            
            % Retreive Harp Data Frame from OSC message
            rxHarpDataFrame = OscToHarpDataFrame(OscRxMessage);
            
            % Check if it's the expected frame
            if FilterHarpDataFrame(rxHarpDataFrame, 34, 'Event') == 1
                % Save data to buffer
                [frameData frameTimestamp] = ParseHarpDataFrame(rxHarpDataFrame);
                motionData = [motionData; frameData];
                timeData = [timeData; frameTimestamp];
                %              motionData = [motionData;ParseHarpDataFrame(rxHarpDataFrame)];
                if framesReceived == 0
                    tic
                end
                framesReceived = framesReceived + 1;
            end
        end
        
        framesReceived
        
        
        % ======= Process data here =======
        
        
        trigger=0;
        motionData = double(motionData);
        runset(rscount:(rscount+framewinsize-1),:)=motionData;
        timeset(tscount:(tscount+framewinsize-1),1)=timeData;
        
        [trigger, outHistset]=OnlineProcessMotionDataEmdGroupCalibrateFirstActionJT51918(motionData,ProcessInput, Library, targetcluster, outHistset,state,oHcount, gyrobaseline, rotMatrix);
        
        oHcount=oHcount+1;
        
        elapsed(epcount,1)=toc;
        
        rscount=rscount+framewinsize;
        tscount=tscount+framewinsize;
        epcount=epcount+1;
    end
    
    m=i;
    
    %% Manipulation VTA stimulation
    
    state=1;
    i = 0; % UPDATE RESET i
    
    while i <= (numstimulationcycles1-1)
        framesReceived = 0;
        
        % Create/clear array of data
        motionData = [];
        timeData=[];
        
        while framesReceived < 60
            % Wait for packet
            [OscRxMessage, sourceHost] = ReceiveRxDatagram(WearRxSocket, rxPacket);
            
            % Retreive Harp Data Frame from OSC message
            rxHarpDataFrame = OscToHarpDataFrame(OscRxMessage);
            
            % Check if it's the expected frame
            if FilterHarpDataFrame(rxHarpDataFrame, 34, 'Event') == 1
                % Save data to buffer
                [frameData frameTimestamp] = ParseHarpDataFrame(rxHarpDataFrame);
                motionData = [motionData; frameData];
                timeData = [timeData; frameTimestamp];
                %              motionData = [motionData;ParseHarpDataFrame(rxHarpDataFrame)];
                if framesReceived == 0
                    startfirstaction = tic;
                end
                framesReceived = framesReceived + 1;
            end
        end
        
        framesReceived;
        
        
        % ======= Process data here =======
        %need to filter data, generate z.GA, x.accel, x.Gyro, log(TotalBA)
        %neeed to generate histogram based on features
        %need to compare feature pattern to an exemplar
        %if match criteria, trigger laser
        % for i=1:10
        trigger=0;
        motionData = double(motionData);
        runset(rscount:(rscount+framewinsize-1),:)=motionData;
        timeset(tscount:(tscount+framewinsize-1),1)=timeData;
        
        [trigger, outHistset]=OnlineProcessMotionDataEmdGroupCalibrateFirstActionJT51918(motionData,ProcessInput, Library, targetcluster, outHistset,state,oHcount, gyrobaseline, rotMatrix);
        
        
        
        % UPDATE ADD THIS SECTION FOR TWO ACTION
        
        if trigger==1
            k=0;
            state = 2;
            kcount = 0;
            
            oHcount=oHcount+1;
            
            elapsed(epcount,1)=toc(startfirstaction);
            
            rscount=rscount+framewinsize;
            tscount=tscount+framewinsize;
            epcount=epcount+1;
            
            i=i+1;
            
            while k == 0
                kcount = kcount+1 %update
                framesReceived = 0;
                
                % Create/clear array of data
                motionData = [];
                timeData=[];
                
                while framesReceived < 60
                    % Wait for packet
                    [OscRxMessage, sourceHost] = ReceiveRxDatagram(WearRxSocket, rxPacket);
                    
                    % Retreive Harp Data Frame from OSC message
                    rxHarpDataFrame = OscToHarpDataFrame(OscRxMessage);
                    
                    % Check if it's the expected frame
                    if FilterHarpDataFrame(rxHarpDataFrame, 34, 'Event') == 1
                        % Save data to buffer
                        [frameData frameTimestamp] = ParseHarpDataFrame(rxHarpDataFrame);
                        motionData = [motionData; frameData];
                        timeData = [timeData; frameTimestamp];
                        %              motionData = [motionData;ParseHarpDataFrame(rxHarpDataFrame)];
                        if framesReceived == 0
                            startsecondaction=tic;
                        end
                        framesReceived = framesReceived + 1;
                    end
                end
                
                framesReceived;
                
                motionData = double(motionData);
                
                runset(rscount:(rscount+framewinsize-1),:)=motionData;
                timeset(tscount:(tscount+framewinsize-1),1)=timeData;
                
                
                [secondtrigger, outHistset, state]=OnlineProcessMotionDataEmdGroupTwoActionCALIBRATEDJT51819(motionData,ProcessInput, Library, targetcluster, secondtargetcluster, outHistset,state, oHcount, gyrobaseline, rotMatrix);
                
                
                if secondtrigger==1
                    % Send OSC message that triggers the Laser
                    SendTxDatagram(MultiPwmTxSocket, txRemoteHost, MultiPwmTxPort, TriggerLaser);
                    k=1;
                    elapsed(epcount,1)=toc(startsecondaction);
                    if i==(numstimulationcycles1-1)
                    elseif i<(numstimulationcycles1-1)
                        i=i+1;
                    end
                    
                elseif secondtrigger==0
                    elapsed(epcount,1)=toc(startsecondaction);
                    if i==(numstimulationcycles1-1)
                        k=1;
                    elseif i<(numstimulationcycles1-1)
                        i=i+1;
                    end
                end
                
                oHcount=oHcount+1;
                
                rscount=rscount+framewinsize; % UPDATE
                tscount=tscount+framewinsize;% UPDATE
                
                epcount=epcount+1;% UPDATE
                
                secondtrigger=0;
                
            end
            
        elseif trigger==0
            oHcount=oHcount+1;
            
            elapsed(epcount,1)=toc(startfirstaction);
            
            rscount=rscount+framewinsize;
            tscount=tscount+framewinsize;
            epcount=epcount+1;
            
            i=i+1;
        end
    end
end


%% Send OSC message to start acquisition
SendTxDatagram(WearTxSocket, txRemoteHost, WearTxPort, StopAcquisition);



%% Close both OSC sockets
CloseDatagramSocket(WearRxSocket);
CloseDatagramSocket(MultiPwmTxSocket);
CloseDatagramSocket(WearTxSocket);

%% Save results

% save(expt);
save(expt,'baselinetime1','Calibrate','gyrobaseline','gyrobaselinestruct','i','k','Library','m','numbaselinecycles1','numcycles','numstimulationcycles1','rotMatrix','outHistset','runset','sensorid','stationarysensor','stimulationtime1','x','y','z','xCalib','yCalib','zCalib','timeset','elapsed','targetcluster', 'secondtargetcluster','rscount','tscount','epcount','oHcount', 'Calibrate','sensorid','gyrobaseline', 'stationarysensor')

%% clear non-filled positions in outHistset

outHistset(find(sum(outHistset,2)==0),:)=[];