/*
* $Id$
*/

/**
* TowerofHanoi.c is part of the Pointer Tutorial for ROBOTC
*
* Changelog:
* - 0.1: Initial release
*
* License: You may use this code as you wish, provided you give credit where it's due.
*
* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.55 AND HIGHER.
* Xander Soldaat (xander_at_botbench.com)
* 20 January 2013
* version 0.1
*/

#define MAX_PEG_SIZE 4                // Maximum number of elements in the Stack

#define isEmpty(X)    (X->size == 0)
#define isFull(X)     (X->size == MAX_PEG_SIZE)

// Individual stack element struct
struct tStackElement
{
  int value;                            // value held by element
} tStackElement;

// Stack struct to hold the elements
struct tStack
{
  tStackElement elements[MAX_PEG_SIZE]; // array of elements
  int size;                               // current size of the stack
} tStack;

// The three pegs;
tStack leftPeg, centrePeg, rightPeg;

bool push(tStack *stack, int value);
int pop(tStack *stack);
void initStack(tStack *stack);
void drawPegs(int floatingDisc = -1);
void hanoi( int N, tStack *source, tStack *dest, tStack *aux );

/**
* Push a new value onto the top of the stack.
* @param stack the stack to manipulate
* @param value the value to be pushed onto the stack
* @return true if a node with this value was pushed succesfully, false if it was not found
*/
bool push(tStack *stack, int value)
{
  if (isFull(stack))
  {
    writeDebugStreamLine("Stack already at max size");
    return false;
  }

  stack->elements[stack->size++].value = value;
  return true;
}


/**
* Pop the last element and return its value
* @param stack the stack to manipulate
* @return the value to be popped off the stack
*/
int pop(tStack *stack)
{
  if (isEmpty(stack))
  {
    writeDebugStreamLine("Stack empty");
    return 0;
  }

  // Decrement size and pop and value from the stack
  return stack->elements[--stack->size].value;
}


/**
* Initialise the stack
* @param stack the stack to manipulate
*/
void initStack(tStack *stack)
{
  // reset the size of the stack
  stack->size = 0;
  memset(stack->elements, 0, sizeof(stack->elements));
}


/**
* Draw the disc we're moving from one peg to another
* @param size the size of the disc to draw
*/
void drawFloatingDisk(int size)
{
  nxtFillRect(48 - (size * 2), 54,  52 + (size * 2), 50);
}


/**
* Draw a disc in the specified position
* @param size the size of the disc to draw
* @param pos the position of the disc on the peg
* @param peg the peg to draw the discs on
*/
void drawDisk(int size, int pos, int peg) {
  int center = 20 + peg * 30;
  nxtFillRect(center - (size * 2) - 2, 14 + (pos * 6),  center + (size * 2) + 2, 10 + (pos * 6));
}


/**
* Draw the three pegs
* @param floatingDisc disc we're currently moving
*/
void drawPegs(int floatingDisc)
{
  // Start with a clean canvas
  eraseDisplay();

  // Draw the sticks on which the rings are placed
  // and label them
	nxtFillRect(19, 40, 21, 10);
	nxtFillRect(49, 40, 51, 10);
	nxtFillRect(79, 40, 81, 10);
	nxtDisplayTextLine(7, "   1    2    3");

	// Draw the floating disc if specified
  if (floatingDisc > 0)
    drawFloatingDisk(floatingDisc);

  // Draw the leftPeg peg
  for (int i = 0; i < leftPeg.size; i++)
    drawDisk(leftPeg.elements[i].value, i, 0);

  // Draw the rightPeg peg
  for (int i = 0; i < rightPeg.size; i++)
    drawDisk(rightPeg.elements[i].value, i, 2);

  // Draw the centrePeg peg
  for (int i = 0; i < centrePeg.size; i++)
    drawDisk(centrePeg.elements[i].value, i, 1);

  // Wait a bit so you can actually see
  wait1Msec(500);
}


// Code taken from http://wiki.answers.com/Q/Towers_of_Hanoi_code_in_c_language_using_stacks
// and modified
void hanoi( int N, tStack *source, tStack *dest, tStack *aux ) // move N discs from source to destination
{
  // This is the disc we're moving from one peg to another
  int disc = 0;

  if (N > 0 )
  {
    //move n-1 disc from source to auxxilary (sub problem)
    hanoi(N - 1, source, aux, dest);

    // Draw the pegs before the move
    drawPegs();

    // Take the disc off the source peg
    disc = pop(source);

    // Draw the pegs during the move
    drawPegs(disc);

    // Put the disc on the destination peg
    push(dest, disc);

    // Draw the pegs after the move
    drawPegs();
    PlaySound(soundBlip);

    //move n-1 discs from auxillary to destination (sub problem)
    hanoi(N - 1, aux, dest, source);
  }
}


task main()
{
  // Initialise the stacks
  initStack(&leftPeg);
  initStack(&centrePeg);
  initStack(&rightPeg);

  // Put a ring on it, or a couple
  for (int i = MAX_PEG_SIZE; i > 0; i--)
    push(&leftPeg, i);

  // Solve the problem recursively
  hanoi(MAX_PEG_SIZE, &leftPeg, &rightPeg, &centrePeg);

  // Draw the final result
  drawPegs();

  // Play a little sound and wait for a button press
  PlaySound(soundDownwardTones);
  nxtDisplayCenteredTextLine(0, "Done!");
  nxtDisplayCenteredTextLine(1, "Press key to exit");
  while(nNxtButtonPressed == kNoButton)
    EndTimeSlice();
}