Arduino Pong

#include <VGA.h>
#include "pitches.h"

int ballX = 240;
int ballY = 240;
int p1X;
int p1YTop;
int p1YBottom;
int p2X;
int p2YTop;
int p2YBottom;
int xVel;
int yVel;
int WHEEL_ONE_PIN= A0;
int WHEEL_TWO_PIN= A1;
int wheelOnePosition;
int wheelTwoPosition;
int paddleWidth = 50;
int velocityX = 1;
int velocityY = -1;
int p1Score = 0;
int p2Score = 0;
int RESET_PIN = 40;
char p1ScoreChar[2];
char p2ScoreChar[2];
String str2;
String str;


//Tone section
// timers TC0 TC1 TC2   channels 0-2 ids 0-2  3-5  6-8     AB 0 1
// use TC1 channel 0
#define TONE_TIMER TC1
#define TONE_CHNL 0
#define TONE_IRQ TC3_IRQn

// TIMER_CLOCK4   84MHz/128 with 16 bit counter give 10 Hz to 656KHz
//  piano 27Hz to 4KHz

static uint8_t pinEnabled[PINS_COUNT];
static uint8_t TCChanEnabled = 0;
static boolean pin_state = false ;
static Tc *chTC = TONE_TIMER;
static uint32_t chNo = TONE_CHNL;

volatile static int32_t toggle_count;
static uint32_t tone_pin;

// frequency (in hertz) and duration (in milliseconds).

void setup() {
VGA.begin(640,480);
pinMode(40, INPUT);
  Serial.begin(9600);

}

void processInputs() {
  VGA.clear();
  str=String(p1Score);
  str2=String(p2Score);
  str2.toCharArray(p2ScoreChar,2);
  str.toCharArray(p1ScoreChar,2);
  VGA.drawText(p1ScoreChar, 20, 460, 1, -256, 0);
  VGA.drawText(p2ScoreChar, 20, 20, 1, -256, 0);
  wheelOnePosition = ((analogRead(WHEEL_ONE_PIN)*640)/1023)-50;
  if (wheelOnePosition>=0){
  VGA.fillRect(wheelOnePosition,35,wheelOnePosition+50,40, 1);
  }else{
   VGA.fillRect(0,35,50,40,1);
  }
   wheelTwoPosition = ((analogRead(WHEEL_TWO_PIN)*640)/1023)-50;
  if (wheelTwoPosition>=0){
  VGA.fillRect(wheelTwoPosition,440,wheelTwoPosition+50,445, 1);
  }else{
    VGA.fillRect(0,440,50,445,1);
  }
  impactCheck();
  ballUpdate();
  VGA.waitSync();
}

void loop() {
  VGA.clear();
    while (digitalRead(RESET_PIN)==1){
    printMenu();
   }
   VGA.clear();
   delay(1000);
   while(digitalRead(RESET_PIN)==1){
    processInputs();
   }
   p1Score = 0;
   p2Score = 0;
   VGA.clear();
   delay(1000);
  VGA.waitSync();
}

void printMenu(){
  VGA.drawText("Press RESET on Side to Begin", 225, 240, 1, 0, 0);
  VGA.drawText("Turn Knobs to Cntrol Paddles", 225, 250, 1, 0, 0);
}

void impactCheck(){
  if ((ballY == 430) && ((ballX>=wheelTwoPosition) && (ballX<=wheelTwoPosition+50))){
    velocityY = -velocityY;
    tone(8, NOTE_G3, 250);
  }
  if ((ballY == 40) && ((ballX>=wheelOnePosition) && (ballX<=wheelOnePosition+50))){
    velocityY = -velocityY;
    tone(8, NOTE_G3, 250);
  }
  if ((ballX == 0) || (ballX==630)){
    velocityX = -velocityX;
  }
  if (ballY == 0){
    tone(8, NOTE_C3, 1000);
    p1Score++;
    ballX = 240;
    ballY = 240;
    VGA.fillEllipse(ballX,ballY,ballX+10,ballY+10,1);
    delay(500);

  }
    if (ballY == 640){
    tone(8, NOTE_C3, 1000);
    p2Score++;
    ballX = 240;
    ballY = 240;
    VGA.fillEllipse(ballX,ballY,ballX+10,ballY+10,1);
    delay(500);
  }
}

void ballUpdate(){
  if (velocityX == 1){
    ballX+=2;
  }else{
    ballX-=2;
  }
  if (velocityY == 1){
    ballY+=2;
  }else{
    ballY-=2;
  }
  VGA.fillEllipse(ballX,ballY,ballX+10,ballY+10,1);
}

void tone(uint32_t ulPin, uint32_t frequency, int32_t duration)
{
    const uint32_t rc = VARIANT_MCK / 256 / frequency;
    tone_pin = ulPin;
    toggle_count = 0;  // strange  wipe out previous duration
    if (duration > 0 ) toggle_count = 2 * frequency * duration / 1000;
    else toggle_count = -1;

    if (!TCChanEnabled) {
      pmc_set_writeprotect(false);
      pmc_enable_periph_clk((uint32_t)TONE_IRQ);
      TC_Configure(chTC, chNo,
        TC_CMR_TCCLKS_TIMER_CLOCK4 |
        TC_CMR_WAVE |         // Waveform mode
        TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC

      chTC->TC_CHANNEL[chNo].TC_IER=TC_IER_CPCS;  // RC compare interrupt
      chTC->TC_CHANNEL[chNo].TC_IDR=~TC_IER_CPCS;
      NVIC_EnableIRQ(TONE_IRQ);
                         TCChanEnabled = 1;
    }
    if (!pinEnabled[ulPin]) {
      pinMode(ulPin, OUTPUT);
      pinEnabled[ulPin] = 1;
    }
    TC_Stop(chTC, chNo);
                TC_SetRC(chTC, chNo, rc);    // set frequency
    TC_Start(chTC, chNo);
}

void noTone(uint32_t ulPin)
{
  TC_Stop(chTC, chNo);  // stop timer
  digitalWrite(ulPin,LOW);  // no signal on pin
}

// timer ISR  TC1 ch 0
void TC3_Handler ( void ) {
  TC_GetStatus(TC1, 0);
  if (toggle_count != 0){
    // toggle pin  TODO  better
    digitalWrite(tone_pin,pin_state= !pin_state);
    if (toggle_count > 0) toggle_count--;
  } else {
    noTone(tone_pin);
  }
}