#include <MeMCore.h>
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>

// Line Finder on PORT_2
MeLineFollower lineFinder_2(PORT_2);
MeRGBLed rgbled_board(7, 2);
MeDCMotor motor_L(9);
MeDCMotor motor_R(10);

int vitesse;
int vitessem;
int droite;
int gauche;

int buttonPressed() {
  return analogRead(A7) <= 10 ? 1 : 0;
}

void mBot_setMotorLeft(int8_t dir, int16_t speed) {
  speed = speed/100.0*255;
  motor_L.run((9) == M1 ? -(dir*speed) : (dir*speed));
}

void mBot_setMotorRight(int8_t dir, int16_t speed) {
  speed = speed/100.0*255;
  motor_R.run((10) == M1 ? -(dir*speed) : (dir*speed));
}


void setup() {
  pinMode(A7,INPUT);
  while (!buttonPressed() ) {}
  vitesse = 50;
  vitessem = 50;
  droite = 0;
  gauche = 0;
}

void loop() {
  if (!lineFinder_2.readSensor2() && !lineFinder_2.readSensor1()) {
    rgbled_board.setColor(1, 0, 0, 0);
    rgbled_board.show();
    rgbled_board.setColor(2, 0, 0, 0);
    rgbled_board.show();
    mBot_setMotorRight(1, vitesse);
    mBot_setMotorLeft(1, vitessem);
  } else if (!lineFinder_2.readSensor1()) {
    rgbled_board.setColor(2, 0, 0, 0);
    rgbled_board.show();
    rgbled_board.setColor(1, 255, 255, 255);
    rgbled_board.show();
    mBot_setMotorRight(1, vitesse);
    mBot_setMotorLeft(0, 0);
    gauche = 1;
    droite = 0;
  } else if (!lineFinder_2.readSensor2()) {
    rgbled_board.setColor(1, 0, 0, 0);
    rgbled_board.show();
    rgbled_board.setColor(2, 255, 255, 255);
    rgbled_board.show();
    mBot_setMotorLeft(1, vitesse);
    mBot_setMotorRight(0, 0);
    gauche = 0;
    droite = 1;
  } else {
    if (!!lineFinder_2.readSensor2() && !!lineFinder_2.readSensor1()) {
      rgbled_board.setColor(1, 255, 255, 255);
      rgbled_board.show();
      rgbled_board.setColor(2, 255, 255, 255);
      rgbled_board.show();
      if (droite == 1) {
        mBot_setMotorRight(1, 0);
        mBot_setMotorLeft(1, vitesse);
      }
      if (gauche == 1) {
        mBot_setMotorRight(1, vitesse);
        mBot_setMotorLeft(1, 0);
      }
    }
  }
}