#include "PixelBotController.h"

#include <Arduino.h>

PixelBotController::PixelBotController()
{
  
}

PixelBotController::~PixelBotController()
{
	
}

unsigned long PixelBotController::GetTicksTravelled()
{
  return ticksTravelled;
}

void PixelBotController::MoveDistance(int direction, int ticks)
{
  for(int i = 0; i < ticks; i++) {
    MoveTick(direction);
    delay(tickDelay);
  }
}

void PixelBotController::MoveTick(int direction)
{
  for(int i = 0; i < 8; i++)
  {
    switch(direction) {
      case STOP:
        return;
      case FORWARDS:
        sendTickPart(i,7-i);
        break;
      case BACKWARDS:
        sendTickPart(7-i,i);
        break;
      case LEFT:
        sendTickPart(7-i,7-i);
        break;
      case RIGHT:
        sendTickPart(i,i);
        break;
    }
    delayMicroseconds(motorSpeed);
  }
  ticksTravelled++;
}

void PixelBotController::sendTickPart(int leftCode, int rightCode)
{
  digitalWrite(lmotorPin1, bitRead(motorLookup[leftCode], 0));
  digitalWrite(lmotorPin2, bitRead(motorLookup[leftCode], 1));
  digitalWrite(lmotorPin3, bitRead(motorLookup[leftCode], 2));
  digitalWrite(lmotorPin4, bitRead(motorLookup[leftCode], 3));
  
  digitalWrite(rmotorPin1, bitRead(motorLookup[rightCode], 0));
  digitalWrite(rmotorPin2, bitRead(motorLookup[rightCode], 1));
  digitalWrite(rmotorPin3, bitRead(motorLookup[rightCode], 2));
  digitalWrite(rmotorPin4, bitRead(motorLookup[rightCode], 3));
}