#include <Arduino_APDS9960.h>

const int Rpins [8] = {2,3,4,5,6,7,8,9};
int etuSensori;
String RX;

int IRinputPin = A0;
int IRinputPin2 = A1;
int IRval = 0;
int IRval2 = 0;
int IRsendPin = 34;
int IRsendPin2 = 44;
volatile bool IRtosi;



// the setup function runs once when you press reset or power the board
void setup() {

  cli(); //Poistetaan käytöstä global interruptit

  TCCR1A = 0; //Alustetaan nämä kaksi keskeistä kontrollirekisteriä
  TCCR1B = 0; // ensin nollaksi

  TCCR1B |= (1 << WGM12); // Tämä bitti päälle ja timer on CTC moodissa
                          // Eli tarkoittaa sitä, että timer
                          // juoksee OCR1A rekisterin lukemaan asti
                          // ja pyörähtää sitten nollaan, josta laskenta jatkuu

  TCCR1B |= (1 << CS10); //Näillä kahdella bitillä B-rekisterissä
  TCCR1B |= (0 << CS12); //asetetaan kellon jako 1024:lla

  OCR1A = 210;        // Vertailurekisteriin laitetaan arvo, johon asti se timer
                        // ehtii laskea yhden sekunnin aikana
                        // näin saadaan yhden sekunnin keskeytystati
                        // 16 000 000 Hz kello jaetaan 1024 = 15625


  TIMSK1 |= (1 << OCIE1A); //Timer HW:sta enabloidaan Output Compare Interrupt Enable

  sei(); //Sallitaan global interrupts

  Serial1.begin(115200);
  Serial.begin(115200);
    while (!Serial);

  if (!APDS.begin()) {
    Serial.println("Error initializing APDS9960 sensor!");
  }
  for(int i = 0; i < 8; i++){
    pinMode(Rpins[i], OUTPUT);
  }
  pinMode(IRinputPin,INPUT);
  pinMode(IRsendPin,OUTPUT);
  pinMode(IRinputPin2,INPUT);
  pinMode(IRsendPin2,OUTPUT);


}
ISR(TIMER1_COMPA_vect){
   IRtosi = !IRtosi;
   digitalWrite(IRsendPin,IRtosi);
   digitalWrite(IRsendPin2,IRtosi);
}

// the loop function runs over and over again until power down or reset
void loop() {

  IRval = analogRead(IRinputPin);
  IRval2 = analogRead(IRinputPin2);
  Serial.println(IRval);
  Serial.println(IRval2);

  RX = Serial1.readStringUntil('\r');
  int RXn = RX.toInt();

  if (APDS.proximityAvailable()) {
    int proximity = APDS.readProximity();
    etuSensori = proximity;
    Serial.println(proximity);
    // print value to the Serial Monitor
    }

  Liikkuminen(RXn, etuSensori);

}

int Liikkuminen(int RXn, int fS){
  if(RXn == 71 && fS > 220){
    ajaEteen();
    }else if(RXn == 71 && fS <= 220){
      pysahdy();
      }

   else if(RXn == 67){
      ajaTaakse();
    }

    else if(RXn == 49){
      ajaVasemmalle();
    }

    else if(RXn == 52){
      ajaOikealle();
    }else{
      pysahdy();
      }

  }

int ajaEteen(){
      digitalWrite(Rpins[0], LOW);
      digitalWrite(Rpins[1], HIGH);
      digitalWrite(Rpins[2], LOW);
      digitalWrite(Rpins[3], HIGH);
      digitalWrite(Rpins[4], LOW);
      digitalWrite(Rpins[5], HIGH);
      digitalWrite(Rpins[6], LOW);
      digitalWrite(Rpins[7], HIGH);
  }
int ajaTaakse(){
      digitalWrite(Rpins[0], HIGH);
      digitalWrite(Rpins[1], LOW);
      digitalWrite(Rpins[2], HIGH);
      digitalWrite(Rpins[3], LOW);
      digitalWrite(Rpins[4], HIGH);
      digitalWrite(Rpins[5], LOW);
      digitalWrite(Rpins[6], HIGH);
      digitalWrite(Rpins[7], LOW);
  }
int ajaVasemmalle(){
      digitalWrite(Rpins[0], LOW);
      digitalWrite(Rpins[1], HIGH);
      digitalWrite(Rpins[2], HIGH);
      digitalWrite(Rpins[3], LOW);
      digitalWrite(Rpins[4], HIGH);
      digitalWrite(Rpins[5], LOW);
      digitalWrite(Rpins[6], LOW);
      digitalWrite(Rpins[7], HIGH);
  }
int ajaOikealle(){
      digitalWrite(Rpins[0], HIGH);
      digitalWrite(Rpins[1], LOW);
      digitalWrite(Rpins[2], LOW);
      digitalWrite(Rpins[3], HIGH);
      digitalWrite(Rpins[4], LOW);
      digitalWrite(Rpins[5], HIGH);
      digitalWrite(Rpins[6], HIGH);
      digitalWrite(Rpins[7], LOW);
  }
int pysahdy(){
      digitalWrite(Rpins[0], LOW);
      digitalWrite(Rpins[1], LOW);
      digitalWrite(Rpins[2], LOW);
      digitalWrite(Rpins[3], LOW);
      digitalWrite(Rpins[4], LOW);
      digitalWrite(Rpins[5], LOW);
      digitalWrite(Rpins[6], LOW);
      digitalWrite(Rpins[7], LOW);
  }