#include "snake.h"
#include "display.h"

unsigned char len = 0;
Body snake[40] = {{0, 0}, 0};


void snake_init()
{
  // Initialize snake
  snake[len].d.x = XXX/2;
  snake[len].d.y = YYY/2;
  snake[len].m = R;

}

void body_move(Body b)
{
  switch(b.m)
    {
    case R:
      b.d.x++;
      break;

    case L:
      b.d.x--;
      break;

    case U:
      b.d.y--;
      break;

    case D:
      b.d.y++;
      break;
    }
}

void snake_move()
{
  int i = 0;
  for(; i <= len; i++)
    {
      //All body parts move
      body_move(snake[i]);
    }
  for(i = 1; i <= len; i++)
    {
      //Sync move direction for next move
      snake[i].m = snake[i - 1].m;
    }
}

void body_add()
{
  // Copy last body part to new body part
	len++;
	snake[len].m = snake[len - 1].m;
	snake[len].d.x = snake[len - 1].d.x;
	snake[len].d.y = snake[len - 1].d.y;

  // Judge the direction of new body part
	switch(snake[len].m)
		{
		case R:
			snake[len].d.x--;
			break;

		case L:
			snake[len].d.x++;
			break;

		case U:
			snake[len].d.y++;
			break;

		case D:
			snake[len].d.y--;
			break;
		}
}

void snake_draw()
{
	int i=1;
	sAddDot(snake[0].d.x,snake[0].d.y);
	sAddDot(snake[0].d.x+1,snake[0].d.y);
	sAddDot(snake[0].d.x,snake[0].d.y+1);
	sAddDot(snake[0].d.x-1,snake[0].d.y);
	sAddDot(snake[0].d.x,snake[0].d.y-1);
	
	for(;i<=len;i++)
	{
		sAddDot(snake[i].d.x,snake[i].d.y);
	}
}

void snake_test()
{
  //Add Body to test
  int i = 0;
  for(; i < 2; i++)
		body_add();
}