// Dette program kan forbinde en Arduino + USB Host Shield + Motor Shield
// Programmet er lavet til at fjernstyre en bil. Blinke med lamper samt dytte
// ved hjælp af en buzzer
// Der bruges en Xbox controller


#include <XBOXRECV.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"


// Laver instancer af vores MotorShield samt vores 2 motorer.
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);

// Laver instancer af vores Usb samt Xbox controller
USB Usb;
XBOXRECV Xbox(&Usb);

// variabler
int pulse = 0; //Hastighed
int piezoPin = 40; //buzzer

void setup() {

  // definerer pins som output til LED

  pinMode(30, OUTPUT); //venstre baglys rød
  pinMode(31, OUTPUT); //højre baglys rød
  pinMode(32, OUTPUT); //venstre blink bag
  pinMode(33, OUTPUT); //højre blink bag

  pinMode(46, OUTPUT); //højre lys foran rød
  pinMode(47, OUTPUT); //venstre forlys rød
  pinMode(48, OUTPUT); //højre blink foran
  pinMode(49, OUTPUT); //højre blink foran

  // Starter Motorshield
  AFMS.begin();

  //Timer
  TCCR1B = TCCR1B & 0b11111000 | 0x02;

  // sætter data rate til 115200 bps
  Serial.begin(115200);

  // Hvis den ikke er forbundet
  if (Usb.Init() == -1)
  {
    Serial.print(F("\r\n Did not start"));
  }

Serial.print(F("\r\nXbox Wireless Receiver Library Started"));

}

void loop() {

  drivingLights();
  Usb.Task();

  if(Xbox.XboxReceiverConnected)
    {

    for(uint8_t i=0;i<4;i++) {

       // R2 Knap
       if(Xbox.getButtonPress(R2, i))
        {
        Serial.print("R2: ");
        Serial.println(Xbox.getButtonPress(R2, i));
        pulse = Xbox.getButtonPress(R2, i);

          if(pulse < 101)
          {
            myMotor->setSpeed(pulse);
            myMotor->run(FORWARD);
          }

          if(pulse < 100)
          {
            myMotor->run(RELEASE);
          }

        }

        // L2 Knap
        if(Xbox.getButtonPress(L2, i))
          {
            Serial.print("L2: ");
            Serial.println(Xbox.getButtonPress(L2, i));
            pulse = Xbox.getButtonPress(L2, i);

          if(pulse <101)
            {
              myMotor->setSpeed(pulse);
              myMotor->run(BACKWARD);
            }

          if(pulse < 99)
            {
              myMotor->run(RELEASE);
            }

          }

          // Højre analog over 7500
          if(Xbox.getAnalogHat(RightHatX, i) > 7500)
            {
              Serial.print(F("RightHatX: "));
              Serial.print(Xbox.getAnalogHat(RightHatX, i));
              Serial.print("\n");
              turnRight();

             }

          // Højre analog under -7500
          if(Xbox.getAnalogHat(RightHatX, i) < -7500)
            {
              Serial.print(F("RightHatX: "));
              Serial.print(Xbox.getAnalogHat(RightHatX, i));
              Serial.print("\n");
              turnLeft();

            }

          //Knap B - Horn
          if(Xbox.getButtonPress(B, i))
            {
             horn();
            }

          //Knap L1 - Blink til venstre
          if(Xbox.getButtonPress(L1, i))
            {
             blinkV();
            }

          //Knap R1 - Blink til højre
          if(Xbox.getButtonPress(R1, i))
            {
             blinkH();
            }

          // Knap Y - Havari Blink
          if(Xbox.getButtonPress(Y, i))
            {
             havariBlink();
            }

      }
   }
}