#include "qfixSoccerBoard.h"


static int speedMotor0 = 0;
static int speedMotor1 = 0;
//static int speedMotor2 = 0;
//static int speedMotor3 = 0;
//static int speedMotor4 = 0;
//static int speedMotor5 = 0;

static unsigned long timerVal = 0;
static unsigned long sleepVal = 0;

static int beepFreq = 0;
static int beepFreqTimer = 0;
static long beepTimer = 0;


// PWM routine //
SIGNAL (SIG_OVERFLOW0)
{
  const int OFFSET=50;    // motor does not work with very low ratio
  static int counter=255+OFFSET;
  
  if (speedMotor0==0)                          cbi(PORTB, 3); // enable1 = 0
  else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 3); // enable1 = 1
  else                                         cbi(PORTB, 3); // enable1 = 0

  if (speedMotor1==0)                          cbi(PORTG, 2); // enable2 = 0
  else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTG, 2); // enable2 = 1
  else                                         cbi(PORTG, 2); // enable2 = 0

  /*if (speedMotor2==0)                          cbi(PORTB, 4); // enable3 = 0
  else if (abs(speedMotor2)+OFFSET >= counter) sbi(PORTB, 4); // enable3 = 1
  else                                         cbi(PORTB, 4); // enable3 = 0

  if (speedMotor3==0)                          cbi(PORTB, 6); // enable4 = 0
  else if (abs(speedMotor3)+OFFSET >= counter) sbi(PORTB, 6); // enable4 = 1
  else                                         cbi(PORTB, 6); // enable4 = 0

  if (speedMotor4==0)                          cbi(PORTB, 5); // enable5 = 0
  else if (abs(speedMotor4)+OFFSET >= counter) sbi(PORTB, 5); // enable5 = 1
  else                                         cbi(PORTB, 5); // enable5 = 0

  if (speedMotor5==0)                          cbi(PORTB, 7); // enable6 = 0
  else if (abs(speedMotor5)+OFFSET >= counter) sbi(PORTB, 7); // enable6 = 1
  else                                         cbi(PORTB, 7); // enable6 = 0*/
  
  if (counter==0) counter=255+OFFSET;
  else counter--;  

  if(timerVal > 0) timerVal--;
  if(sleepVal > 0) sleepVal--;

  if(beepTimer > 0) beepTimer--;

  if(beepTimer == 0) cbi(PORTB, 4);
  else {
    if(beepFreqTimer) beepFreqTimer--;
        else beepFreqTimer = beepFreq;

    if(beepFreqTimer > beepFreq/2) sbi(PORTB, 4);
        else cbi(PORTB, 4);
  }
}

void initTimer()
{
  TIMSK0=1;		// timer 0 overflow for interrupt (8 bit timer)
  TCCR0A=2;		// prescaler 8 => 0.5us
  TCNT0=56;
  sei();    // enable interrupts
}


SoccerBoard::SoccerBoard()
{
  // PORT A: Digital In (PA0, PA1) //
  DDRA= 255 - (1+2);   			// all bits output (motor direction) except PA0,PA1 (digital in)
  PORTA=1+2;           				// set pullups for digital in

  // PORT B:  LEDs (PB0, PB2), Motor enable (PB3-PB7) //
  DDRB  = 1+4;                     	// PB0 + PB4 = LEDs -> output
  DDRB |= 8+16+32+64+128;      	// PB3 - PB7 = Motor enable -> output
  PORTB |= 1+4;             		// set bits 0 and 2 -> LEDs off
  PORTB &= 255-(8+16+32+64+128); 	// clear bits 3-7 -> motor disable

  // PORT C: Power Output //
  DDRC  = 255;             			// direction port D, all bits output
  PORTC = 0;               			// clear all bits -> power on

  // PORT D: I2C, USB, CAN //
  DDRD  = 0;                 		// all bits input
  PORTD = 1+2;               		// set bits 0,1 -> I2C pullUps

  // PORT E: Digital In (PE2 - PE7) //
  DDRE=0;		              		// all bits input
  PORTE=4+8+16+32+64+128;    		// set pullups for digital in

  // PORT F: Analog In //
  DDRF=0;		   					// all bits input
  ADCSRA=128;              			// set A/D enable bit (ADEN)

  // PORT G: Buttons (PG3, PG4), motor enable (PG2)
  DDRG = BV(PG2);            // PG2 output
  PORTG= BV(PG3)+BV(PG4);    // set pullups for buttons
  cbi(PORTG,PG2);            // clear PG2 -> motor disable

  initTimer();
}


void SoccerBoard::ledOn(int i)
{
  if (i==0) cbi(PORTB, PB0);         // clear bit -> LED on
  else if (i==1) cbi(PORTB, PB2);	  // clear bit -> LED on  
}


void SoccerBoard::ledOff(int i)
{
  if (i==0) sbi(PORTB, PB0);         // set bit -> LED off
  else if (i==1) sbi(PORTB, PB2);	  // set bit -> LED off  
}

void SoccerBoard::ledsOff()
{
  PORTB|=BV(PB0)+BV(PB2);          // set bits -> LEDs off
}

void SoccerBoard::led(int i, bool state)
{
  if (state) ledOn(i); else ledOff(i);
}


void SoccerBoard::powerOn(int i)
{
  if ((i<0) || (i>7)) return;
  cbi(PORTC, i);
}


void SoccerBoard::powerOff(int i)
{
  if ((i<0) || (i>7)) return;
  sbi(PORTC, i);
}


void SoccerBoard::power(int i, bool state)
{
  if (state) powerOn(i); else powerOff(i);
}


bool SoccerBoard::button(int i)
{
  if (i==0) return ( (PING & BV(PG4)) == 0);
  else if (i==1) return ( (PING & BV(PG3)) == 0);
  else return false;    // bad approach...
}


void SoccerBoard::ledMeter(int i,int maxvalue = 256)
{
  //led(0, (i>100)); //modified by meyma
  //led(1, (i>200));

  if(i < (maxvalue / 4))
  {
        ledOff(0);
    ledOff(1);
  }
  else if(i < (maxvalue / 2))
  {
        ledOn(0);
    ledOff(1);
  }
  else if(i < ((maxvalue /4) * 3))
  {
    ledOff(0);
    ledOn(1);
  }
  else {
    ledOn(0);
        ledOn(1);
  }
}

void SoccerBoard::motor(int i, int speed)
{
  if ((i<0) || (i>1)) return;

  if (i==0) {
    speedMotor0 = speed;
    if (speed>0) sbi(PORTA, 3);   // input1  = 1
    else         cbi(PORTA, 3);   // input1  = 0
  }

  else if (i==1) {
    speedMotor1 = speed;
    if (speed>0) sbi(PORTA, 2);   // input2  = 1
    else         cbi(PORTA, 2);   // input2  = 0
  }

  /*else if (i==2) {
    speedMotor2 = speed;
    if (speed>0) sbi(PORTA, 5);   // input3  = 1
    else         cbi(PORTA, 5);   // input3  = 0
  }

  else if (i==3) {
    speedMotor3 = speed;
    if (speed>0) sbi(PORTA, 4);   // input4  = 1
    else         cbi(PORTA, 4);   // input4  = 0
  }

  else if (i==4) {
    speedMotor4 = speed;
    if (speed>0) sbi(PORTA, 7);   // input5  = 1
    else         cbi(PORTA, 7);   // input5  = 0
  }

  else if (i==5) {
    speedMotor5 = speed;
    if (speed>0) sbi(PORTA, 6);   // input6  = 1
    else         cbi(PORTA, 6);   // input6  = 0
  }*/
}

void SoccerBoard::motorsOff()
{
  motor(0,0);
  motor(1,0);
  motor(2,0);
  motor(3,0);
  motor(4,0);
  motor(5,0);
}


// return 0-255 //
int SoccerBoard::analog(int i)
{
  if ((i<0) || (i>7)) return -1;

  ADMUX=i;                       // select analog input and start A/D
  sbi(ADMUX, ADLAR);             // left adjust -> we use only ADCH
  sbi(ADCSRA, ADSC);             // start conversion
  while (ADCSRA & 64);           // wait until ADSC is low again
  int value = ADCH;              // read 8 bit value fom ADCH
  return value;
}


bool SoccerBoard::digital(int i)
{
  if ((i<0) || (i>7)) return false;  // bad solution...

  if (i==0) return (PINA & 1);
  else if (i==1) return (PINA & 2);
  else return (PINE & (1<<i)) ;
}


void SoccerBoard::waitForButton(int i)
{
  if ((i<0) || (i>3)) return;  // bad solution...
  while (!button(i))  { /* do nothing */ }
  while (button(i))   { /* do nothing */ }
}

void SoccerBoard::beep(int freq) {
  beep(freq, -1);
}

void SoccerBoard::beep(int freq, long msecs) {
  beepTimer = 7*msecs;
  beepFreq = 7000 / freq;
}
  
void SoccerBoard::beepOff() {
  beepTimer = 0;
}

void SoccerBoard::setTimer(unsigned long msecs) {
  timerVal = 7*msecs;
}

bool SoccerBoard::timer() {
  return (timerVal <= 0);
}

void SoccerBoard::sleep(unsigned long msecs) {
  sleepVal = 7*msecs;

  while(sleepVal > 0);
}