#include <Arduino.h>
#include "motor.h"

char sensorpins[] = {13, 14, 27, 26, 25, 33, 32};
int values[7];
int average;
int threshold = 500;

Motor left(22, 23, 0);
Motor right(19, 21, 1);
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  // pinMode(22, OUTPUT);
  // pinMode(23, OUTPUT);
  // pinMode(19, OUTPUT);
  // pinMode(21, OUTPUT);
  // digitalWrite(22, LOW);
  // digitalWrite(23, LOW);
  // digitalWrite(19, LOW);
  // digitalWrite(21, LOW);
  left.forward(255);
  right.forward(255);
}

void loop() {
  // put your main code here, to run repeatedly:
  static int leftspeed = 0;
  static int rightspeed = 0;
  for(int i=0;i<7;i++) {
    values[i] = analogRead(sensorpins[i]);  
    average += values[i];
    average >>= 1;
    //Serial.printf("%i ", values[i]);
    if(values[i] > average + threshold ) {
      Serial.print(" 1 ");
    } else {
      Serial.print(" 0 ");
    }
  }

  // if(values[0] > average + threshold) {
  //   right.forward(63);
  //   left.forward(0);
  // }
  // // if(values[1] > average + threshold) {
  // //   right.forward(255);
  // //   left.forward(63);
  // // }
  // // if(values[2] > average + threshold) {
  // //   right.forward(255);
  // //   left.forward(127);
  // // }
  // if(values[3] > average + threshold) {
  //   left.forward(63);
  //   right.forward(63);
  // }
  // // if(values[4] > average + threshold) {
  // //   left.forward(63);
  // //   right.forward(32);
  // // }
  // // if(values[5] > average + threshold) {
  // //   left.forward();
  // //   right.forward(63);
  // // } 
  // if(values[6] > average + threshold) {
  //   left.forward(63);
  //   right.forward(0);
  // }

  
  Serial.printf("\r");
  //delay(1000);
 
  
}