/**
 * @file 
 * File containing bestroute module
 */
#include "bestroute.h"

/* Start function definition of bestroute module */

unsigned int bestroute( unsigned int *doorsRow,
                        unsigned int *doorsCol,
                        unsigned int numOfDoor,
                        unsigned int *doorId
                      )
{
  D1(printf("\nBESTROUTE.bestroute() - START\n"));
  D4(printf("\nParameters in: %u %u %u %u %u\n",
                              schemaData.numRow,
                              schemaData.numCol,
                            schemaData.startRow,
                            schemaData.startCol,
                                       numOfDoor  ));

  /* Return value. Default assignament (row * column + 1) 
                   is impossible to obtain by computation. */
  unsigned int bestRouteRet = schemaData.numRow * schemaData.numCol + 1;
  
  if(!bestroute_check_in_data(doorsRow,doorsCol,numOfDoor)) //Something's wrong
      return bestRouteRet;

  unsigned int *vectRow = (unsigned int *)malloc( sizeof(unsigned int)*
                                       (schemaData.numRow*schemaData.numCol) );
  unsigned int *vectCol = (unsigned int *)malloc( sizeof(unsigned int)*
                                       (schemaData.numRow*schemaData.numCol) );
  long int *vectParent = (long int *)malloc( sizeof(long int)*
                                       (schemaData.numRow*schemaData.numCol) );

  vectRow[0] = schemaData.startRow;
  vectCol[0] = schemaData.startCol;
  vectParent[0] = -1;
  
  unsigned int numOfNodes = schema_round_search(vectRow, vectCol, vectParent);

  unsigned int i = 0, j = 0;

  for( i=0; i < numOfDoor; i++ ) // for all the doors
  {
      assert( i < numOfDoor );

      bool found = false;
      for( j=0; ( j <= numOfNodes ) && ( ! found ); j++ ) // for all nodes
      {
          assert( (j <= numOfNodes ) && (!found) );

          D4(printf("\nIs a door: (%u,%u) == (%u,%u)?",
                  vectRow[j],vectCol[j],doorsRow[i],doorsCol[i] ));	

          if( ! ( ( vectRow[j] == doorsRow[i] ) &&
                  ( vectCol[j] == doorsCol[i] )     )
            )
              continue; // it isn't a door

          assert( ( vectRow[j] == doorsRow[i] ) &&
                  ( vectCol[j] == doorsCol[i] )
                );
                                                  
          //No other doors equals to i-esim
          found = true;

          unsigned int lenRoute = reverse_route_length( vectRow,    vectCol, 
                                                        vectParent, j       );

          D2(printf("\ndoor = %u , route weigth= %u",i,lenRoute));

          if(lenRoute<bestRouteRet){
              bestRouteRet = lenRoute;
              *doorId = i;
          }
      }
  }

  free( vectRow );
  free( vectCol );
  free( vectParent );

  D1(printf("\nBESTROUTE.bestroute() - END\n"));

  return bestRouteRet;
}


bool bestroute_check_in_data( const unsigned int *doorsRow,
                              const unsigned int *doorsCol,
                                    unsigned int numOfDoor
                            )
{

    D1(printf("\nBESTROUTE.bestroute_check_in_data() - START\n"));
  
    if( schemaData.numRow > MAXR ){
        D1(printf("\nSchema rows exceed maximum allowed (%u)\n",MAXR));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert( ! (schemaData.numRow > MAXR) );

    if( schemaData.numRow < MINR ){
        D1(printf("\nSchema rows lower then minimum allowed (%u)\n",MINR));
        D1(printf("\nBESTROUTE.bestroute() - Error & END\n"));
        return false;
    }
    assert( ! ( schemaData.numRow < MINR ) );

    if( schemaData.numCol > MAXC ){
        D1(printf("\nSchema columns exceed maximum allowed (%u)\n",MAXC));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert( ! ( schemaData.numCol > MAXC ) );

    if( schemaData.numCol < MINC ){
        D1(printf("\nSchema columns lower then minimum allowed (%u)\n",MINC));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert( ! ( schemaData.numCol < MINC ) );

    const unsigned short int INSUFFICIENT_CELL_NUMBER = 2;
    if( (schemaData.numRow * schemaData.numCol) < INSUFFICIENT_CELL_NUMBER ){
        D1(printf("\nNot enough cell to set a START point and a DOOR\n"));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert(! ((schemaData.numRow*schemaData.numCol)<INSUFFICIENT_CELL_NUMBER));

    const unsigned short int INSUFFICIENT_DOOR_NUMBER = 1;
    if( numOfDoor < INSUFFICIENT_DOOR_NUMBER ){
        D1(printf("\nNeed at least one DOOR\n"));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert( ! ( numOfDoor < INSUFFICIENT_DOOR_NUMBER ) );

    unsigned int it = 0;
    for(it=0; it<numOfDoor; it++){
        if( (doorsRow[it] >= schemaData.numRow ) ||
            (doorsCol[it] >= schemaData.numCol )
          )
        {
           D1(printf("\nDOOR %u (%u,%u) out of schema\n",
                             it,doorsRow[it],doorsCol[it] ));
           D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
           return false;
        }
        assert( ! ( (doorsRow[it] >= schemaData.numRow ) ||
                    (doorsCol[it] >= schemaData.numCol )    )
              );
    }

    if( (schemaData.startRow >= schemaData.numRow) ||
        (schemaData.startCol >= schemaData.numCol)
      )
    {
        D1(printf("\nSTART point out of schema\n"));
        D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
        return false;
    }
    assert( ! ( (schemaData.startRow >= schemaData.numRow) ||
              (schemaData.startCol >= schemaData.numCol)      )
          );

    for(it=0; it<numOfDoor; it++){
        if( (doorsRow[it] == schemaData.startRow ) &&
            (doorsCol[it] == schemaData.startCol )
          )
        {
           D1(printf("\nDOOR %u (%u,%u) has the same coordinates of the"
                   ,it,doorsRow[it],doorsCol[it]));
           D1(printf(" START POINT (%u,%u)\n"
                  ,schemaData.startRow, schemaData.startCol));
           D1(printf("\nBESTROUTE.bestroute_check_in_data() - Error & END\n"));
           return false;
        }
        assert( ! ( (doorsRow[it] == schemaData.startRow ) &&
                    (doorsCol[it] == schemaData.startCol )     )
              );
    }

    D1(printf("\nBESTROUTE.bestroute_check_in_data() - END\n"));
   
    // All controls ok
    return true;
}

unsigned int schema_round_search( unsigned int *vectRow,
                                  unsigned int *vectCol,
                                  long int *vectParent
                                )
{

  D1(printf("\nBESTROUTE.schema_round_search() - START\n"));
  
  unsigned int i = 0, j = 0;
  
  bool mapCopy [ schemaData.numRow ] [ schemaData.numCol ];
  
  for( i=0; i < schemaData.numRow; i++ )
      for( j=0; j < schemaData.numCol; j++ )
          mapCopy[i][j] = isaccessible( schemaData.gameMap[i][j] );

  unsigned int findNodes = 0;
  i = 0;
  
  while( i <= findNodes)
  {
      assert( i <= findNodes );
      D3(printf("\nWhile at %u of (%u)\n",i,(findNodes+1)));
      D3(printf("\nnode %u %u %li\n",vectRow[i],vectCol[i],vectParent[i]));

      if( ( vectCol[i] > 0 ) &&
          ( mapCopy[vectRow[i]][vectCol[i]-1] ) )
      {
          findNodes++;
          vectRow[findNodes] = vectRow[i];
          vectCol[findNodes] = vectCol[i]-1;
          vectParent[findNodes] = i;

          D5( printf("\nSX = schemaData.gameMap[%u][%u] = %c (int %i)",
                       vectRow[i],
                       (vectCol[i]-1),
                       schemaData.gameMap[vectRow[i]][vectCol[i]-1],
                       schemaData.gameMap[vectRow[i]][vectCol[i]-1] 
                    )
             );

          mapCopy[vectRow[i]][vectCol[i]-1] = false;
      }

      if( ( vectRow[i] > 0 ) &&
          ( mapCopy[vectRow[i]-1][vectCol[i]] ) )
      {
          findNodes++;
          vectRow[findNodes] = vectRow[i]-1;
          vectCol[findNodes] = vectCol[i];
          vectParent[findNodes] = i;

          D5( printf("\nUP = schemaData.gameMap[%u][%u] = %c (int %i)",
                      (vectRow[i]-1),
                      vectCol[i],
                      schemaData.gameMap[vectRow[i]-1][vectCol[i]],
                      schemaData.gameMap[vectRow[i]-1][vectCol[i]] 
                    )
             );

          mapCopy[vectRow[i]-1][vectCol[i]] = false;
      }

      if( ( vectCol[i] < (schemaData.numCol-1) ) &&
          ( mapCopy[vectRow[i]][vectCol[i]+1]  )     )
      {
          findNodes++;
          vectRow[findNodes] = vectRow[i];
          vectCol[findNodes] = vectCol[i]+1;
          vectParent[findNodes] = i;

          D5( printf("\nDX = schemaData.gameMap[%u][%u] = %c (int %i)",
                      vectRow[i],
                      (vectCol[i]+1),
                      schemaData.gameMap[vectRow[i]][vectCol[i]+1],
                      schemaData.gameMap[vectRow[i]][vectCol[i]+1] 
                    )
              );

          mapCopy[vectRow[i]][vectCol[i]+1] = false;
      }

      if( ( vectRow[i] < (schemaData.numRow-1) ) &&
          ( mapCopy[vectRow[i]+1][vectCol[i]]  )     )
      {
          findNodes++;
          vectRow[findNodes] = vectRow[i]+1;
          vectCol[findNodes] = vectCol[i];
          vectParent[findNodes] = i;

          D5( printf("\nDW = schemaData.gameMap[%u][%u] = %c (int %i)",
                      (vectRow[i]+1),
                      vectCol[i],
                      schemaData.gameMap[vectRow[i]+1][vectCol[i]],
                      schemaData.gameMap[vectRow[i]+1][vectCol[i]] 
                    )
             );

          mapCopy[vectRow[i]+1][vectCol[i]] = false;
      }

      D3(printf("\nUpdates neighbours at iteration %u: %u\n",i,findNodes));

      i++;
  }

  D1(printf("\nBESTROUTE.schema_round_search() - END\n"));
  
  return findNodes;
}


unsigned int reverse_route_length( unsigned int *vectRow,
                                   unsigned int *vectCol,
                                   long int  *vectParent,
                                   long int doorIndex
                                 )
{

  D1(printf("\nBESTROUTE.reverse_route_length() - START\n"));

  unsigned int lenRoute = 0;

  D2(printf("\nEND->"));

  do        // reverse index on vectParent (parent) in 'log(n)'
  {
      lenRoute++;

      D2(
              const unsigned int lineToShow = 10;
              if( ( lenRoute % lineToShow ) == 0 )
                printf("\n");
              printf("(%u,%u)->",vectRow[doorIndex],vectCol[doorIndex]); 
        );

      doorIndex = vectParent[ doorIndex ];

  }
  while( doorIndex != -1 );

  D2( printf("START\n") );

  D1(printf("\nBESTROUTE.reverse_route_length() - END\n"));
  
  return lenRoute;
}
/* End function definition of bestroute module */
