#include "..\main.h"

//#define SLEEP_LONG 2.5
//#define PLACE_VICINITY_DIST 100


#define MINIMUM_JUMP_DIST 50
#define NEXT_ROAD_DIST    250  // Search for free road piece

#if 1

#define TIME_UNTIL_STUCK_CHECK  60
#define STUCK_BELOW_DIST  70  // Half of base lenght (FOB_MAX_SIZE)
#define JUMP_SLEEP        SLEEP_LONG

#else // If debug settings

#define TIME_UNTIL_STUCK_CHECK  1
#define STUCK_BELOW_DIST  710
#define JUMP_SLEEP 1

#endif

#define INF_JUMP_DISTANCE (MINIMUM_JUMP_DIST + 300)  // For inf
#define SEEK_ROAD_DIST 450    // For vehicles

#define ENEMY_NEAR_DISTANCE_JUMP 500

#define JUMP_EFFECT 1

lastJumpPos = [0,0,0];

jumpOnStuck =
{
 params ["_group"];

 sleep 5; // For testing mostly but little delay here is ok

 while { runLoops && _group call isGroupAlive } do
 {
  // No point to run if no movers
 if(_group call isGroupSpawned) then
 {

  _vehs = _group call getVehicles;
   // Move only moving vehicles
  _vehs = _vehs select { (getNumber (configfile >> "CfgVehicles" >> (typeof _x) >> "maxSpeed")) > 0 };

  _target = _group call getGroupNextMovePos;

  _jumped = false;



  //["Test moving %1", _vehs] call dbgmsg;

  {
   _vehMan = _x;
   _isMan = _vehMan isKindOf "man";
  _vpos = getposATL _vehMan;

  // Vehicle or on foot
  if((!_isMan || (vehicle _vehMan == _vehMan)) ) then
  {

   // format["Time to stuck %1 ",time - (_vehMan getVariable "lastMoveTime")] call dbgmsg;

  if((time - (_vehMan getVariable ["lastMoveTime",0])) > TIME_UNTIL_STUCK_CHECK) then
  {


   // Jump only moving not fully spawned groups
   // If not jumpong and not in target and not moved much - jump
  if( (_group getVariable ["isMoving",false]) && (_group call isGroupMinSpawned)
  &&  { !(_vehMan getVariable ["jumping",false]) && (_target distance2D _vpos) > PLACE_VICINITY_DIST && (_vehMan getVariable "lastPos") distance2D _vpos < STUCK_BELOW_DIST } ) then
  {
   ["one vehicle/man (%1) stuck! %2 -- %3",_isMan,_group,_vehMan] call dbgmsg;

   _moved = false;

   if(_isMan) then
   {
   _moved = [_group,_vehMan,_target,(_vehMan getVariable ["timesStuck",0])] call getPosNearestToTarget;
   }
   else
   {
   _moved = [_group,_vehMan,_target,(_vehMan getVariable ["timesStuck",0])] call getRoadNearestToTarget;

   if(_moved) then
   {

    // Move all infantry with the vehicle (shouldnt be needed with minimal spawn)
   {
    _man = _x;
    if(vehicle _man == _man) then
    {
    _man setVariable ["timesStuck",0];
    _man setVariable ["lastPos", _vpos];
    _man setVariable ["lastMoveTime",time];

     _vec = [random 360, 12] call getVector;
     _manSetPos = [lastJumpPos,_vec] call addVector;
     _manSetPos set [2,0];
      _man setposATL _manSetPos;
    };
   } foreach (units _group);

   };


   };

   if(!_moved) then
   {
   _vehMan setVariable ["timesStuck", (_vehMan getVariable ["timesStuck",0]) + 1];
   }
   else
   {
    _vehMan setVariable ["timesStuck",0];
   };

   if(_moved) then
   {
    _jumped = true;
   };
  }
  else
  {
   // systemchat format ["time took to move %1", time - _time];

   _vehMan setVariable ["timesStuck",0];
  };


   _vehMan setVariable ["lastPos", _vpos];
   _vehMan setVariable ["lastMoveTime",time];
  };

  };

  } forEach (_vehs + (units _group));


 // Try to continue movement if jumped
 if(_jumped) then
 {
 [_group,leader _group] spawn continueMoving;
 };
 };

 sleep JUMP_SLEEP;
};
};



getPosNearestToTarget =
{
 params ["_group","_man","_target","_timesStuck"];

 _side = side _group;

 _spos = getposATL _man;

 // Todo --> if([_pos,_side,_eneDist] call isEnemyNear)

 _angle = [_spos,_target] call getAngle;
 _vec = [_angle,INF_JUMP_DISTANCE] call getVector;

 _jpos = [_spos,_vec] call addvector;

_freepos = [_jpos, 0, 50, 0.5 , 0, 0.9, 0, [], [[0,0],[0,0]] ] call BIS_fnc_findSafePos;

_jumped = false;

if(_freepos call isValidCoord) then // Actually found anything?
{

if(!([_freepos,_side,ENEMY_NEAR_DISTANCE_JUMP * 0.7] call isEnemyNear)) then
{

 _freepos set [2,0];

 if(isnull _man) exitWith {}; // Just in case

 [_man,_freepos] spawn jumpToPos;

 _jumped = true;
};

};

 _jumped
};


getRoadNearestToTarget =
{
 params ["_group","_veh","_target","_timesStuck"];
/*
if(side _group == west) then
{
["getRoadNearestToTarget 333 ::: %1 %2 %3", _group, leader _group, _veh] call dbgmsg;
};*/

private _side = side _group;

private _pos = getposATL _veh;

private _eneDist = ENEMY_NEAR_DISTANCE_JUMP;
if(_timesStuck >= 10) then
{
 _eneDist = _eneDist * 0.5;
} else {
if(_timesStuck >= 3) then
{
 _eneDist = _eneDist * 0.7;
};
};

if([_pos,_side,_eneDist] call isEnemyNear) exitWith { ["Can't jump enemy near %1",_group] call dbgmsg; false }; // can't jump if enemy near


private _nearRoads = [_pos] call searchForRoads;

//["enenear 3"] call dbgmsg;


_closestRoad = objNull;
_closestDist = 10000000;

_dbgRoadSpot = false;
_dbgEnemySpot = false;

{
 private _road = _x;

 // Don't jump too short
 if((_road distance2D _pos) > (MINIMUM_JUMP_DIST + NEXT_ROAD_DIST)) then
 {
  _dbgRoadSpot = true;
 if(!([getposATL _road,_side,_eneDist] call isEnemyNear)) then
 {
  _dbgEnemySpot = true;

 private _dist = _road distance2D _target;
 if(_dist < _closestDist) then
 {
  _closestDist = _dist;
  _closestRoad = _road;
 };
 };
 };
} foreach _nearRoads;

private _foundPos = false; // Dbg

if(!isnull _closestRoad) then
{
 // systemchat format["Moving to %1", _closestRoad];

 _mpos = getposatl _closestRoad;


 private _nearRoads = _mpos nearRoads NEXT_ROAD_DIST;

 //_nextVeh = 0;
 while { count _mpos > 0 } do
 {
  scopename "getNewPos";

  // _veh = _vehs select _nextVeh;

 _eobjs = _mpos nearObjects 10; // Todo use spawn function
 if(count _eobjs == 0) then
 {

  ["Moving vehicle %1 %2 %3 = %4 (%5)", _group, leader _group, _veh, crew _veh, _group call getgrouptype] call dbgmsg;

  // systemchat "moving one vehicle";
  _mpos set[2,0.5];

  lastJumpPos = _mpos;

  //_veh setposAtl _mpos;
  if(isnull _veh) then { break; }; // Just in case

  [_veh,_mpos] spawn jumpToPos;



  // Todo move infantry in when jumping

_dir = _mpos call getRoadDir;
if(_dir >= 0) then
{
_veh setdir _dir;
};

[_veh] domove ([_mpos,_target] select (vehicle (leader _group) == _veh)); // must do or vehicle starts moving back

 // [_mpos,"","ColorRed"] call createDebugMarker;

  //_nextVeh = _nextVeh + 1;

  _foundPos = true;
  breakout "getNewPos";
 };

 _mpos = [];

 // Get next road pos if this one was blocked
 if(count _nearRoads > 0) then
 {
 _mpos = getPosAtl (_nearRoads select 0);
 _nearRoads deleteat 0;
 };

};

}
else
{
 ["Unable to find jump center road pos %1 %2 %3", _dbgRoadSpot, _dbgEnemySpot, count _nearRoads] call dbgmsg;
};

if(!_foundPos) then { ["Could not get free jump pos! %1 -- %2",_group,_veh] call dbgmsg; };

 _foundPos
};



jumpToPos =
{
scopename "jumpTo";
params ["_unit","_toPos"];

// diag_log format ["jumpToPos: %1 %2 %3 %4 %5", side _unit, _unit,(_unit iskindof "man"), (_unit getVariable ["jumping",false]), _toPos];

if(!alive _unit) then { breakout "jumpTo"; };

if(_unit getVariable ["jumping",false]) exitwith {}; // Make sure this happens only once

private _isMan = (_unit iskindof "man");

_esize = 1;
if(_unit iskindof "man") then
{
 _esize = 0;
};

dostop _unit;

_unit setVariable ["jumping",true];

// [_toPos,"jump",["ColorRed","ColorBlue"] select (_unit iskindof "man") ] call createDebugMarker;




#if JUMP_EFFECT
[ _unit, true, _esize ] remoteExecCall ["BIS_fnc_VRSpawnEffect", call targetClients];

//[ _unit, true, 1 ] call BIS_fnc_VRSpawnEffect;

sleep 5;

if(!alive _unit) then { breakout "jumpTo"; };

#endif

// Make sure man has not gone in to a vehicle
// If vehicle or man on foot (never move man in vehicle)
if(!_isMan || !(_unit call inVehicle) ) then
{


private _freePos = [_toPos,SPAWN_VEH_SPACE,0,50] call findSafePosVehicle;

if(count _freePos > 0) then
{
 _toPos = _freePos;
};

[_unit, _toPos] call setPosSafe;

/*
if(side _unit == west && !_isMan && testmode) then
{
 _toPos set [0,(_toPos # 0) + 5];
 player setposATL _toPos;
};*/


#if JUMP_EFFECT

sleep 5;

if(!alive _unit) then { breakout "jumpTo"; };

[ _unit, false, _esize ] remoteExecCall ["BIS_fnc_VRSpawnEffect", call targetClients];

#endif

};

_unit setVariable ["jumping",false];

};