//
//  Tower of Hanoi 
//          by Sakaki Engineering
//

#define MAX_DISC 10

#define WIDTH 64
#define BYTE_WIDTH (WIDTH / 4)
#define HEIGHT 16
#define ARY_SIZE (BYTE_WIDTH * HEIGHT)
#define POLE_DIST 21
#define POLE_POS(x) (x * POLE_DIST + POLE_DIST / 2 + 1)

// Outpu port
#define LED_RED_DATA0   2  // Red LED serial data
#define LED_GREEN_DATA0 3  // Red LED serial data
#define LED_RED_DATA1   4  // Red LED serial data
#define LED_GREEN_DATA1 5  // Red LED serial data
#define BIT_RED0   0x04
#define BIT_GREEN0 0x08
#define BIT_RED1   0x10
#define BIT_GREEN1 0x20

#define LED_SCLK  10   // Serial data clock
#define LED_LATCH 11   // Latch 1 row data
#define LED_RESET 12   // Reset to first row

#define INPUT_VR 0

// LED color
#define LED_OFF 0
#define LED_RED 1
#define LED_GREEN 2
#define LED_ORANGE 3

#define BASE_COLOR LED_GREEN
#define POLE_COLOR LED_RED
#define DISC_COLOR LED_ORANGE
#define BACK_COLOR LED_OFF

// LED data buffer
byte LedData[ARY_SIZE];

// number of discs
int disc;
// disc number for each pole
int discNum[3];
// disc size for each axe
byte discLayout[3][MAX_DISC];

// Delay of display
int delayTime;

//
//  Setup port and data
//

void setup()
{
  int i;
  int j;
  
  // Initialize output pins
  pinMode(LED_RESET, OUTPUT);  
  pinMode(LED_LATCH, OUTPUT);  
  pinMode(LED_SCLK, OUTPUT);  
  pinMode(LED_RED_DATA0, OUTPUT);  
  pinMode(LED_GREEN_DATA0, OUTPUT);
  pinMode(LED_RED_DATA1, OUTPUT);  
  pinMode(LED_GREEN_DATA1, OUTPUT);

  digitalWrite(LED_RESET, HIGH);
  digitalWrite(LED_LATCH, HIGH);
  digitalWrite(LED_SCLK, HIGH);

  // Initialize data
  delayTime = 2;
  for (i = 0; i < 3; i++) {
    discNum[i] = 0;
    for (j = 0; j < MAX_DISC; j++) {
      discLayout[i][j] = 0;
    }
  }

  // Clear LED buffer
  clear(BACK_COLOR);

  // Draw base plate and tree poles
  drawBase();
  
  // Set discs
  setDiscNum();

  return;
}

//
//  Clear data buffer
//

void clear(byte c)
{
  int i;
  byte t;

  t = (c << 6) | (c << 4) | (c << 2) | c;
  for (i = 0; i < ARY_SIZE; i++) {
    LedData[i] = t;
  }
  return;
}

//
// Set number of discs
//

void setDiscNum()
{
  int notEnd;
  int i, j;
  int old;
  unsigned long lastTime, crntTime;

  notEnd = true;
  lastTime = millis();
  old = 0;
  while (notEnd) {
    crntTime = millis();

    // Sense VR
    disc = analogRead(INPUT_VR) / 80 + 1;
    if (disc > 10) {
      disc = 10;
    }  
    if (disc != old) {
      // Get new value
      discNum[0] = disc;
      old = disc;
      j = disc;
      for (i = 0; i < disc; i++) {
        discLayout[0][i] = j--;
      }

      // Update LED image
      j = disc;
      for (i = 0; i < MAX_DISC; i++) {
        eraseDisc(POLE_POS(0), i, 10);
        drawDisc(POLE_POS(0), i, j--);
      }
      lastTime = crntTime;
    }

    // Check time out
    if (crntTime >= (lastTime + 5000)) {
      notEnd = false;
    }
    displayLed();
  }
  return;
}

//
//  Main loop
//

void loop()
{
  int i;

  // Move 0 to 2
  hanoi(0, 2, 1, disc);
  for (i = 0; i < 100; i++) {
    displayLed();
  }
  invLed();
  for (i = 0; i < 150; i++) {
    displayLed();
  }
  invLed();
  for (i = 0; i < 100; i++) {
    displayLed();
  }
  // Move 2 to 0
  hanoi(2, 0, 1, disc);
  for (i = 0; i < 100; i++) {
    displayLed();
  }
  invLed();
  for (i = 0; i < 150; i++) {
    displayLed();
  }
  invLed();
  for (i = 0; i < 100; i++) {
    displayLed();
  }

  return;
}

//
//  Hanoi
//

void hanoi(int src, int dst, int tmp, int cnt)
{
  if (cnt > 0) {
    hanoi(src, tmp, dst, cnt - 1);
    moveDisc(src, dst);
    hanoi(tmp, dst, src, cnt - 1);
  }
  return;
}

//
//  Move disc
//

void moveDisc(int s, int d)
{
  int i, x, y;
  int size;
  int srcx, dstx;
  int hs, hd;

  // Update disc data
  hs = --discNum[s];
  size = discLayout[s][hs];
  discLayout[s][hs] = 0;

  hd = discNum[d]++;
  discLayout[d][hd] = size;

  //  Disc animation
  srcx = POLE_POS(s);
  dstx = POLE_POS(d);

  // move disc upward
  for (y = hs; y < 12; y++) {
    drawDisc(srcx, y + 1, size);
    eraseDisc(srcx, y, size);
    for (i = 0; i < delayTime; i++) {
      displayLed();
    }
  }

  // horizontal
  if (s > d) {
    // to left
    for (x = srcx; x > dstx; x--) {
      eraseDisc(x, 12, size);
      drawDisc(x - 1, 12, size);
      for (i = 0; i < delayTime; i++) {
        displayLed();
      }
    }
  } else {
    // to right
    for (x = srcx; x < dstx; x++) {
      eraseDisc(x, 12, size);
      drawDisc(x + 1, 12, size);
      for (i = 0; i < delayTime; i++) {
        displayLed();
      }
    }
  }

  // downward
  for (y = 12; y > hd; y--) {
    drawDisc(dstx, y - 1, size);
    eraseDisc(dstx, y, size);
    for (i = 0; i < delayTime; i++) {
      displayLed();
    }
  }

  return;
}

//
//  Draw disc
//

void drawDisc(int p, int y, int size)
{
  int i;

  if (size < 1) {
    return;
  }
  for (i = p - size; i <= (p + size); i++) {
    setLed(i, 13 - y, DISC_COLOR);
  }
  return;
}

//
//  Erase disc
//

void eraseDisc(int p, int y, int size)
{
  int i;

  for (i = p - size; i <= (p + size); i++) {
    setLed(i, 13 - y, BACK_COLOR);
  }
  if (y < 11) {  // Draw pole
    setLed(p, 13 - y, POLE_COLOR);
  }
  return;
}

//
//  Draw base and poles
//

void drawBase()
{
  int x;

  for (x= 0; x < 3; x++) {
    drawPole(POLE_POS(x));
  }

  for (x = 0; x < WIDTH; x++) {
    setLed(x, 14, BASE_COLOR);
    setLed(x, 15, BASE_COLOR);
  }
  return;
}

void drawPole(int x)
{
  int i;

  for (i = 3; i < 14; i++) {
    setLed(x, i, POLE_COLOR);
  }  
  return;
}

//
//  Control one LED
//

void setLed(int x, int y, int c)
{
  int p;
  int shift;
  byte t;
  byte mask;

  p = y * BYTE_WIDTH + (x >> 2);
  shift = 2 * (x & 0x03);
  mask = ~(0x03 << shift);
  t = c << shift;
  LedData[p] = (LedData[p] & mask) | t;

  return;
}

void invLed()
{
  int i;

  for (i = 0; i < ARY_SIZE; i++) {
    LedData[i] = ~LedData[i];
  }
  return;
}

//
//  Diplay LED panel (1 cycle only)
//

void displayLed()
{
  int x, y;
  byte t0, t1;
  byte d;
  int cnt;

  uint8_t bit0, bit1;

  cnt = 0;

  for (y = 0; y < HEIGHT; y++) {
    for (x = 0; x < WIDTH / 2; x++) {
      if ((x & 0x03) == 0) {
        t0 = LedData[cnt];
        t1 = LedData[cnt + BYTE_WIDTH / 2];
        cnt ++;
      }
      // LED data clock and LED data
      digitalWrite(LED_SCLK, LOW);  // SCLK low
      d = PORTD;
      if (t0 & 0x01) {
        d |= BIT_RED0;
      } else {
        d &= ~BIT_RED0;
      }
      if (t0 & 0x02) {
        d |= BIT_GREEN0;
      } else {
        d &= ~BIT_GREEN0;
      }
      t0 >>= 2;
      if (t1 & 0x01) {
        d |= BIT_RED1;
      } else {
        d &= ~BIT_RED1;
      }
      if (t1 & 0x02) {
        d |= BIT_GREEN1;
      } else {
        d &= ~BIT_GREEN1;
      }
      t1 >>= 2;
      PORTD = d;
      digitalWrite(LED_SCLK, HIGH);  // SCLK high

      if ((x == 0) && (y == 0)) {
        // Reset row number
        digitalWrite(LED_RESET, LOW);
        digitalWrite(LED_RESET, HIGH);
      }
    }
    // Row data latch
    digitalWrite(LED_LATCH, LOW);
    digitalWrite(LED_LATCH, HIGH);
    cnt += BYTE_WIDTH / 2;
  }
  return;
}