// ---------------------------------------------------------------------
// SATMS BOT - Simplified Assumption-Based Truth Maintenance System BOT
// ---------------------------------------------------------------------


#include "core"
#include "math"
#include "bots"


// ----------------------------------------------------------
// Particular BOT knowledge
// ----------------------------------------------------------
// (id) Assumptions
// ~~~~~~~~~~~~~~~~~~~~~~~~~
// (0)  "no danger"
// (1)  "enemies shooting"
// (2)  "enemy in range"
// (3)  "dangerous bullet coming"
// ----------------------------------------------------------
// (id) Observations
// ~~~~~~~~~~~~~~~~~~~~~~~~~
// (0)  hearing enemy shots
// (1)  not hearing enemy shots
// (2)  enemies in sight
// (3)  no enemies in sight
// (4)  enemy bullet approaching
// (5)  enemy bullet in your direction
// ----------------------------------------------------------
// (id) Actions
// ~~~~~~~~~~~~~~~~~~~~~~~~~
// (0)  walk()
// (1)  movesHeadAround()
// (2)  watchForWalls()
// (3)  checkWarriorCollision()
// (4)  correctBodyAlignment()
// (5)  aimLastSpottedEnemy()
// (6)  shootEnemy()
// (7)  doRun()
// (8)  avoidEnemyBullet()
// ----------------------------------------------------------

// Inconsistent environments (nogoods)
// {0, 1}, {0, 2}, {0, 3}, {0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {0, 1, 2, 3}


// Basic constants
new const ENEMY_WARRIOR = ITEM_ENEMY | ITEM_WARRIOR;
new const FRIEND_WARRIOR = ITEM_FRIEND | ITEM_WARRIOR;
new const ENEMY_GUN = ITEM_ENEMY | ITEM_GUN
new const ENEMY_BULLET = ITEM_ENEMY | ITEM_BULLET
new float:headDir = 1.047;

// Advanced constants
const NUM_OBS = 6;
const NUM_ASSUMPTIONS = 4;
const NUM_ACTIONS = 9;


// TABLES
// ----------------------------------------------------------------------------
// Tells which observations are true
new bool:observationsState[NUM_OBS] = {false, ...};

// Tells which assumptions are true
new bool:assumptionsState[NUM_ASSUMPTIONS] = {false, ...};

// Tells which actions should be executed
new bool:actionsToDo[NUM_ACTIONS] = {false, ...};

// For each assumption, where the assumption's id is represented by an array line, each line
// contains the following: {n, x1, ..., xn}, where n indicates the number of
// observations needed to activate this assumption and the other x1, ..., xn numbers are
// the observations's ids.
new const REL_AOBS[NUM_ASSUMPTIONS][] = {{2, 1, 3}, {1, 0}, {1, 2}, {1, 5}};

// For each action, where the action's id is represented by an array line, each line
// contains the following: {n, x1, ..., xn}, where n indicates the number of
// assumptions needed in an OR fashion to activate this action 
// and the other x1, ..., xn numbers are the assumptions's ids.
new const REL_ACTASS_OR[NUM_ACTIONS][] = {{1, 0}, {1, 0}, {1, 0}, {3, 0, 1, 2}, {1, 0}, {2, 1, 2}, {1, 2}, {1, 2}, {1, 3}};

// For each action, where the action's id is represented by an array line, each line
// contains the following: {n, x1, ..., xn}, where n indicates the number of
// assumptions needed in an AND fashion to activate this action 
// and the other x1, ..., xn numbers are the assumptions's ids.
new const REL_ACTASS_AND[NUM_ACTIONS][] = {{0}, {0}, {0}, {0}, {0}, {0}, {0}, {0}, {0}};


// -------------------------------------------------------
// NOTE: the environments are encoded as the example below
// -------------------------------------------------------
// ASSUMPTIONS's IDs: 3 2 1 0
// ON/OFF:            1 0 0 1
// CODE:              9
// ENVIRONMENT:       {0, 3}
//
// hint: it uses binary base.
// -------------------------------------------------------


// For each possible environment, each line has the following meaning: {n, x1, ..., xn}, where
// n indicates de number of consistent subsets of the current environment, and the x1, ..., xn
// indicates the environment's codes of the consistent subsets.
new const REL_MAXCONS[16][] = {{0}, {0}, {0}, {2, 1, 2}, {0}, {2, 1, 4}, {2, 2, 4}, {1, 6}, {0}, {2, 1, 8}, {2, 2, 8}, {1, 10}, {2, 4, 8}, {1, 12}, {3, 6, 10, 12}, {1, 14}};

// Inconsistent environments (nogoods)
new const NOGOOD[16] = {0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1};
// ----------------------------------------------------------------------------


// Dynamic information variables
// ----------------------------------------------------------------------------
new float:lastEnemyYaw = 0.0;
new float:lastEnemyPitch = 0.0;
new lastEnemyItem = 0;

new float:lastEnemyBulletYaw = 0.0;
new float:lastEnemyBulletPitch = 0.0;
new lastEnemyBulletItem = 0;

new float:avoidingModifier = 1.0;

new decodeVector[NUM_ASSUMPTIONS] = {0, ...};
// ----------------------------------------------------------------------------


// Sensors implementations
// ----------------------------------------------------------------------------
hearEnemyShots() {
   new sound;
   new item = ENEMY_GUN;
   new float:yaw;
   new float:pitch;
   
   hear(item, sound, yaw, pitch);
   
   if (item == ENEMY_GUN) {
      observationsState[0] = true;
      lastEnemyItem = item;
      lastEnemyYaw = yaw;
	  lastEnemyPitch = pitch;
   } else {
      observationsState[1] = true;
   }
}

lookForEnemies() {
   new item = ENEMY_WARRIOR;
   new float:dist = 0.0;
   new float:yaw;
   new float:pitch;
   
   watch(item, dist, yaw, pitch);

   if (item == ENEMY_WARRIOR) {
      observationsState[2] = true;
      lastEnemyItem = item;
      lastEnemyYaw = yaw;
	  lastEnemyPitch = pitch;
   } else {
      observationsState[3] = true;
   }
}

lookForEnemyBullets() {
   new item = ENEMY_BULLET;
   new float:dist = 15.0;
   new float:yaw;
   new float:pitch;
   
   watch(item, dist, yaw, pitch);

   if (item == ENEMY_BULLET) {
      observationsState[4] = true;
      lastEnemyBulletItem = item;
      lastEnemyBulletYaw = getDirection() + getTorsoYaw() + getHeadYaw() + yaw;
	  lastEnemyBulletPitch = pitch;
   }
}

// To calculate the bullet's route, we need to watch it again.
calculateApproachingBulletRoute() {
   new float:dist = 15.0;
   new float:yaw;
   new float:pitch;
   new float:correctedBulletYaw = 0.0;

   if (!observationsState[4]) {
      return;
   }
   
   watch(lastEnemyBulletItem, dist, yaw, pitch);
   
   correctedBulletYaw = getDirection() + getTorsoYaw() + getHeadYaw() + yaw;
   
   if (abs(lastEnemyBulletYaw - correctedBulletYaw) <= 0.01) {
      observationsState[5] = true;
   }
}
// ----------------------------------------------------------------------------


// Actions implementations
// ----------------------------------------------------------------------------
movesHeadAround() {
   rotateHead(headDir);
   
   if (getHeadYaw() == headDir) {
      headDir = -headDir;
   }
}

watchForWalls() {

   if (sight() < 5.0) {
      rotate(getDirection() + 0.31415);
   }
}

checkWarriorCollision() {
 
   if (getTouched() & ITEM_WARRIOR != 0) {
      rotate(getDirection() + 1.5708);
   }
}

doRun() {

   if (isRunning()) {
      return;
   }
   
   if (isCrouched()) {
      stand();
   }
   
   if (isWalking()) {
      run();
   } else {
      walk();
      run();
   }
}    

aimLastSpottedEnemy() {

   if (lastEnemyItem == ENEMY_WARRIOR
      || lastEnemyItem == ENEMY_GUN) {
	  
      rotate(getDirection() + getTorsoYaw() + getHeadYaw() + lastEnemyYaw);
	  bendTorso(lastEnemyPitch);
      bendHead(-1.0 * lastEnemyPitch);
      rotateHead(0.0);
	  rotateTorso(0.0);
   }
}

shootEnemy() {
   aim(lastEnemyItem);

   if (lastEnemyItem != FRIEND_WARRIOR) {
      shootBullet();
   }
}

avoidEnemyBullet() {
   rotate(getDirection() + (avoidingModifier * 1.6));
   rotateTorso(-1.0 * avoidingModifier * 2.6);
   rotateHead(0.0);
   
   avoidingModifier = -1.0 * avoidingModifier;
}

correctBodyAlignment() {
   rotateTorso(0.0);
}
// ----------------------------------------------------------------------------


// ----------------------------------------------------------
// SATMS BOT CORE ENGINE
// ----------------------------------------------------------

cleanObservations() {

   for (new i = 0; i < NUM_OBS; i++) {
      observationsState[i] = false;
   }
}

// DEBUG
printAssumptionsState() {
   new envCode = floatround(environmentCode(), floatround_ceil);

   print("^n--------------------------------------------------------------^n");
   
   print("assumptions: ");
   
   for (new i = NUM_ASSUMPTIONS - 1; i >= 0; i--) {
      printint(assumptionsState[i]);
      print(" ");
   }
   
   print(" envCode: ");
   printint(envCode);
   
   print(" decodeVector: ");
   
   environmentDecode(envCode);

   for (new i = NUM_ASSUMPTIONS - 1; i >= 0; i--) {
      printint(decodeVector[i]);
      print(" ");
   }
   
   print("^n--------------------------------------------------------------^n");
}

// DEBUG
printTroubleSensing() {
   new trueAssumptionsCount = 0;

   for (new i = 0; i < NUM_ASSUMPTIONS; i++) {
      
	  if (assumptionsState[i] == true) {
	     trueAssumptionsCount++;
      }
   }
   
   if (trueAssumptionsCount > 1) {
      print("More than ONE assumption activated!^n");
   }
}

// Read data from the sensors that are required for the observations.
// Activate related assumptions.
sense() {
   cleanObservations();
   
   // sensors reading
   hearEnemyShots();
   lookForEnemies();
   lookForEnemyBullets();
   calculateApproachingBulletRoute();

   // assumptions activation
   for (new i = 0; i < NUM_ASSUMPTIONS; i++) {
      new n = REL_AOBS[i][0];
      new bool:state = true;
	  
      for (new j = 1; j <= n; j++) {
	  
	     if (observationsState[REL_AOBS[i][j]] == false) {
		    state = false;
            break;
		 }
	  }
	  
	  assumptionsState[i] = state;
   }   
   
   // DEBUG
   printAssumptionsState();
   
   // DEBUG
   // printTroubleSensing();
}

float:environmentCode() {
   new float:code = 0.0;
   
   for (new i = 0; i < NUM_ASSUMPTIONS; i++) {
   
      if (assumptionsState[i]) {
         code += pow(2.0, i * 1.0);
      }
   }
   
   return code;
}

environmentDecode(envCode) {
   new tempCode = envCode;
   
   for (new i = 0; i < NUM_ASSUMPTIONS; i++) {
      decodeVector[i] = tempCode % 2;
	  tempCode = tempCode / 2;
   }
}

// SATMS MAGIC!!
// If the environment obtained by sensing is not consistent, finds the maximal consistent
// environment that is a subset of the inconsistent one. If there are more than one, choose
// one randomly.
deliberate() {
   new envCode = floatround(environmentCode(), floatround_ceil);
   new randomChoice = 0;
   
   // nothing to do at single assumption's environments.
   if (envCode == 0 || envCode == 1 || envCode == 2 || envCode == 4 || envCode == 8) {
      return;
   }
   
   if (!NOGOOD[envCode]) {
      return;
   }
   
   print("^nInconsistency detected! Repairing it...^n");
   
   randomChoice = random(REL_MAXCONS[envCode][0]) + 1;
   environmentDecode(REL_MAXCONS[envCode][randomChoice]);
   
   // Environment reconfiguration.
   for (new i = 0; i < NUM_ASSUMPTIONS; i++) {
   
      // this extra code is just to avoid the tag mismatch warning.
      if (decodeVector[i] == 1) {
	     assumptionsState[i] = true;
      } else {
	     assumptionsState[i] = false;
	  }
   }
   
   print("^n^nEnvironment reconfigured to:^n");
   printAssumptionsState();
   print("^n^n");
}

cleanActionsToDo() {

   for (new i = 0; i < NUM_ACTIONS; i++) {
      actionsToDo[i] = false;
   }
}

lookUpActionsToDoInANDFashion() {

   // actions activation
   for (new i = 0; i < NUM_ACTIONS; i++) {
      new n = REL_ACTASS_AND[i][0];
      new bool:state = true;
	  
	  if (actionsToDo[i] == true) {
	     continue;
	  }
	  
      for (new j = 1; j <= n; j++) {
	  
	     if (assumptionsState[REL_ACTASS_AND[i][j]] == false) {
		    state = false;
            break;
		 }
	  }
	  
	  if (n == 0) {
	     actionsToDo[i] = false;
	  } else {
	     actionsToDo[i] = state;
	  }
   }   
}

lookUpActionsToDoInORFashion() {

   // actions activation
   for (new i = 0; i < NUM_ACTIONS; i++) {
      new n = REL_ACTASS_OR[i][0];
      new bool:state = false;
	  
	  if (actionsToDo[i] == true) {
	     continue;
	  }
	  
      for (new j = 1; j <= n; j++) {
	  
	     if (assumptionsState[REL_ACTASS_OR[i][j]] == true) {
		    state = true;
            break;
		 }
	  }
	  
      actionsToDo[i] = state;
   }   
}

lookUpActionsToDo() {
   cleanActionsToDo();
   lookUpActionsToDoInORFashion();
   lookUpActionsToDoInANDFashion();
}

// Before acting, the defined environment should be consistent. An environment consists of
// all true assumptions. For each true assumption, executes the related actions.
act() {
   lookUpActionsToDo();
   
   if (actionsToDo[0]) {
      walk();
   }

   if (actionsToDo[1]) {
      movesHeadAround();
   }  

   if (actionsToDo[2]) {
      watchForWalls();
   }  

   if (actionsToDo[3]) {
      checkWarriorCollision();
   }  

   if (actionsToDo[4]) {
      correctBodyAlignment();
   }  
   
   if (actionsToDo[5]) {
   	  aimLastSpottedEnemy();
   }  

   if (actionsToDo[6]) {
   	  shootEnemy();
   }  

   if (actionsToDo[7]) {
      doRun();
   }
   
   if (actionsToDo[8]) {
      avoidEnemyBullet();
   }
}      

main() {
   seed(10);

   while (true) {
      sense();
      deliberate();
      act();
   }
}

// ----------------------------------------------------------