%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright (c) 2014, Simon Brodeur
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%
%  - Redistributions of source code must retain the above copyright notice,
%    this list of conditions and the following disclaimer.
%  - Redistributions in binary form must reproduce the above copyright notice,
%    this list of conditions and the following disclaimer in the documentation
%    and/or other materials provided with the distribution.
%  - Neither the name of Simon Brodeur nor the names of its contributors
%    may be used to endorse or promote products derived from this software
%    without specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
% NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
% OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
% WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
%
1;
source ../torcs_opt.m

function pairs = pairSelection(values,iterations)
  pairs = cell(1,iterations);
  sumFitness = sum(cellfun(@double,values(:,2)));

  % Create Roulette
  rouletteValues = zeros(iterations*2, 1);
  for i=1:iterations*2
    rouletteValues(i) = values{i,2}/sumFitness;
  endfor
  rouletteValues = sort(rouletteValues);
  for i=2:iterations*2
    rouletteValues(i) = rouletteValues(i-1) + rouletteValues(i);
  endfor

  for k=1:iterations
    idx1 = find(rouletteValues >= rand())(1);
    idx2 = find(rouletteValues >= rand())(1);
    pairs{k} = [values(idx1,1), values(idx2,1)];
  endfor
endfunction

function crossedPair = crossPairs(pair)
  cutIdx = 4;
  CP = cell(2,1);
  if (rand() > 0.5)
    CP(1,1) = [pair{1}{1}(1:cutIdx), pair{1}{2}(cutIdx+1:end)];
    CP(2,1) = [pair{1}{2}(1:cutIdx), pair{1}{1}(cutIdx+1:end)];
  else
    CP(1,1) = pair{1}{1};
    CP(2,1) = pair{1}{2};
  endif
  crossedPair = CP;
endfunction

function mutantPair = mutatePair(pair, maxValues, minValues)
  mutantPair = pair;
  if (rand() > 0.90)
    % Choose parameter to modify
    bit = ceil(rand()*8);
    %Modify the parameter
    pair(bit) = pair(bit)*(rand()*(0.5 + rand()*1.0));
    if (pair(bit) > maxValues(bit))
      pair(bit) = maxValues(bit);
    elseif (pair(bit) < minValues(bit))
      pair(bit) = minValues(bit);
    endif
    mutantPair = pair;
  endif
endfunction


% Connect to simulation server
startSimulator(mode='nogui');

unwind_protect

  # Generate initial population
  iterations = 40;
  cvalues = cell(iterations,2);

  for i=1:iterations
    cvalues(i,1) = (MAX_PARAM_VALUES - MIN_PARAM_VALUES) .* rand(1, NB_PARAMS) + MIN_PARAM_VALUES;
  endfor

  epochs = 50;
  newValues = cell(iterations,2);
  maxIndiv = 0;
  bestIndiv = cell(1);


  %%% MODE SELECT %%%
  %sportMode = 0; % Fuel Economy
  sportMode = 1; % Performance

  for Z=1:epochs
    % Calculate fitness
    for j=1:iterations
      [result, status] = evaluateParameters(cvalues{j,1}, maxEvaluationTime=1000);
      if (sportMode)
        cvalues{j,2} = result.topspeed;
      else
        cvalues{j,2} = result.distraced/result.fuelUsed;
      endif
    endfor
    % Keep best individual
    testValue = cell2mat(cvalues(:,2));
    [x, ix] = max (testValue);
    if (x > maxIndiv)
      maxIndiv = x
      bestIndiv = cvalues{ix, 1}
    endif

    % Select new pairs
    pairs = pairSelection(cvalues, iterations/2);

    % Cross pairs with each other ( 50% )
    crossedPairs = cell(iterations/2,1);
    for x=1:iterations/2
      tmpPair = crossPairs(pairs(x));
      crossedPairs{x} = tmpPair;
    endfor

    % Mutation ( 10% )
    for y=1:2:iterations/2
      cvalues{y} = mutatePair(crossedPairs(y){1}{1},MAX_PARAM_VALUES,MIN_PARAM_VALUES);
      cvalues{y+1} = mutatePair(crossedPairs(y){1}{2},MAX_PARAM_VALUES,MIN_PARAM_VALUES);
    endfor
  endfor


  stopSimulator();

unwind_protect_cleanup
    % Close the simulator
    stopSimulator();
  disp('All done.');
end_unwind_protect