/*
  Arduino Analoge Modelbaan Sturing Nieuwe versie
  Copyright Paul Smits, 2016

  This sketch use the AT Mega 2650 processorboard

  Warning!!! This sketch work only on the AT Mega 2650

  This Programm use the AAMS Library

  This code is public domain, enjoy!
*/

#include <AAMS.h>
#include <Wire.h>
#include <LCD.h>               // library voor een standaard LCD display.
#include <LiquidCrystal_I2C.h> // library voor 12c LCD display

/***** Global vars ***********************************************/
// Define I2C Address where the PCF8574A is

#define BACKLIGHT_PIN     3 // zet de backlight pen op 3

LiquidCrystal_I2C   lcd(0x27, 2, 1, 0, 4, 5, 6, 7); // adressering lcd display

AAMS set_Motor_1(1, MOTOR12_64KHZ);
AAMS set_Motor_2(2, MOTOR12_64KHZ);
AAMS set_Motor_3(3, MOTOR34_64KHZ);
AAMS set_Motor_4(4, MOTOR34_64KHZ);

// Digitale pinnen benoemen.
byte BlokPins[] = {22, 23, 24, 25, 26, 27 , 28, 29};// Acht blokken
byte WisselPins[] = {30, 31, 32, 33, 34, 35, 36, 37};// Vier wissels
byte SeinPins [] = { 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53};// Acht seinen.

// Analoge pinnen benoemen
byte SensorPins[] = {A8, A9, A10, A11, A12, A13, A14, A15};//Acht sensoren

// Blok toestand
byte Vrij = LOW;
byte Bezet = HIGH;

//Wisselstand
byte Rechtdoor = LOW;
byte Afbuigen = HIGH;

// Adresering Seinen
byte Sein1g = 40;
byte Sein1r = 41;
byte Sein2r = 42;
byte Sein2g = 43;
byte Sein3g = 44;
byte Sein3r = 45;
byte Sein4r = 46;
byte Sein4g = 47;

// Adresering Sensoren
byte Sensor1 = A8;
byte Sensor2 = A9;
byte Sensor3 = A10;
byte Sensor4 = A11;

boolean IgnoreSensor1 = false;
boolean IgnoreSensor2 = false;

//-----------------------------------------------------------------------------
void setup() {

  // put your setup code here, to run once:

  lcd.begin (16, 2);  // stelt de lcd driver in op 16 characters en twee regels.
  // lcd.begin(20,4); // Stelt de lcd driver in op 20 characters en vier regels.

  // set de backlight van het lcd display aan.

  lcd.setBacklightPin(BACKLIGHT_PIN, POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.home (); // Ga naar de boventse regel en het eerste character.

  // Zet het onderstaande tekst op het lcd display

  lcd.setCursor(0, 0); // zet de cursor op het eerste character op de eerste regel
  lcd.print("ArduinoMega 2650");
  lcd.setCursor(1, 1);
  lcd.print("L293 AAMS V1.0"); // Max Ampere is 600 mA per output.

  //**************************************************************
  // Stelt alle pins in die gebruikt worden als input of output.
  // BlokPins instellen en de toestand instellen.

  for (byte i = 0; i < sizeof(BlokPins); i++) {
    pinMode(BlokPins[i], OUTPUT);
  }
  for (byte i = 0; i < sizeof(BlokPins); i++) {
    digitalWrite(BlokPins[i], HIGH);
  }

  // WisselPins instellen.

  for (byte i = 0; i < sizeof(WisselPins); i++) {
    pinMode(WisselPins[i], OUTPUT);
  }

  // SeinPins instellen.

  for (byte i = 0; i < sizeof(SeinPins); i++) {
    pinMode(SeinPins[i], OUTPUT);
  }
  for (byte i = 0; i < sizeof(SeinPins); i++) {
    digitalWrite(SeinPins[i], LOW);
  }

  // Sensor pins instellen.

  for (byte i = 0; i < sizeof(SensorPins); i++) {
    pinMode(SensorPins[i], INPUT);
  }

  // Einde instellingen poorten
  //*****************************************************************
  Serial.begin(9600); // open een serial verbinding op 9600 baud.
  Serial.println("Arduino analoge modelbaan sturing: Motorsturingen en blok sturingen");

  // turn on motor

  set_Motor_1.setSpeed(250);
  set_Motor_2.setSpeed(250);
  set_Motor_3.setSpeed(200);
  set_Motor_4.setSpeed(250);

  set_Motor_1.run(RELEASE);
  set_Motor_2.run(RELEASE);
  set_Motor_3.run(RELEASE);
  set_Motor_4.run(RELEASE);

  digitalWrite(Sein1g, 0);
  digitalWrite(Sein1r, 1);
  digitalWrite(Sein2r, 1);
  digitalWrite(Sein2g, 0);
}

//------------------------------------------------------------
void loop() {

  // put your main code here, to run repeatedly:

  checkIngekomenSerial();

  blok1Bezet();
  //blok2Bezet();

}

//------------------------------------------------------------
void blok1Bezet()
{

  byte Sensor1Waarde = analogRead(Sensor1);
  byte Sensor2Waarde = analogRead(Sensor2);

  if ((Sensor1Waarde <= 60) && (Sensor2Waarde <= 60)) {

    Motor3Stop();

  }
}

//-----------------------------------------------------------
void blok2Bezet()
{

  byte Sensor1Waarde = analogRead(Sensor1);
  byte Sensor2Waarde = analogRead(Sensor2);

  if ((Sensor2Waarde <= 60) && (Sensor1Waarde <= 60)) {

    Motor3Stop();

  }
}

//-------------------------------------------------------------
void Motor1_BACKWARD()
{

  uint8_t i;

  Serial.println("Achteruit met de trein");

  set_Motor_1.run(BACKWARD);

  for (i = 0; i < 200; i++) {
    set_Motor_1.setSpeed(i);
    delay(10);
  }
}
//----------------------------------------------------
void Motor1_FORWARD()
{

  uint8_t i;

  Serial.println("Vooruit met de trein");

  set_Motor_1.run(FORWARD);


  for (i = 0; i < 200; i++) {
    set_Motor_1.setSpeed(i);
    delay(10);
  }
}

//----------------------------------------------------------
void Motor2_BACKWARD()
{

  uint8_t i;

  Serial.println("Achteruit met de trein");

  set_Motor_2.run(BACKWARD);

  for (i = 0; i < 200; i++) {
    set_Motor_2.setSpeed(i);
    delay(10);
  }
}

//-----------------------------------------------------------
void Motor2_FORWARD()
{

  uint8_t i;

  Serial.println("Vooruit met de trein");

  lcd.setCursor(1, 1);
  lcd.print("Vooruit trein_1");

  set_Motor_2.run(FORWARD);


  for (i = 0; i < 200; i++) {
    set_Motor_2.setSpeed(i);
    delay(20);
  }
}

//-------------------------------------------------------------
void Motor3_BACKWARD()
{

  uint8_t i;

  Serial.println("Achteruit met de trein");

  lcd.setCursor(0, 1);
  lcd.print(" Trein 3 >>   ");

  set_Motor_3.run(FORWARD);

  for (i = 0; i < 200; i++) {
    set_Motor_3.setSpeed(i);
    delay(20);

    lcd.setCursor(13, 1);
    lcd.print(i);

  }
}

//-------------------------------------------------------------
void Motor3_FORWARD()
{

  uint8_t i;

  Serial.println("Vooruit met de trein");

  lcd.setCursor(0, 1);
  lcd.print(" Trein 3 <<   ");

  set_Motor_3.run(BACKWARD);


  for (i = 0; i < 200; i++) {
    set_Motor_3.setSpeed(i);
    delay(15);

    lcd.setCursor(13, 1);
    lcd.print(i);

  }
}

//-------------------------------------------------
void Motor3_Stop()
{
  uint8_t i;

  Serial.println("Stoppen met de trein");

  lcd.setCursor(0, 1);
  lcd.print(" Trein 3 ||  ");

  for (i = 200; i != 0; i--) {
    set_Motor_3.setSpeed(i);
    delay(5);

    lcd.setCursor(13, 1);
    lcd.print(i);

  }
  set_Motor_3.run(RELEASE);
}
//--------------------------------------------------

void Motor4_BACKWARD()
{
  uint8_t i;

  Serial.println("Achteruit met de trein");

  set_Motor_4.run(BACKWARD);

  for (i = 0; i < 200; i++) {
    set_Motor_4.setSpeed(i);
    delay(10);
  }
}
//----------------------------------------------------
void Motor4_FORWARD()
{

  uint8_t i;

  Serial.println("Vooruit met de trein");

  set_Motor_4.run(FORWARD);

  for (i = 0; i < 200; i++) {
    set_Motor_4.setSpeed(i);
    delay(10);
  }
}

//-------------------------------------------------
void Motor4_Stop()
{
  uint8_t i;

  Serial.println("Stoppen met de trein");

  lcd.setCursor(0, 1);
  lcd.print(" Trein 4 ||  ");

  for (i = 250; i != 0; i--) {
    set_Motor_4.setSpeed(i);
    delay(10);

    lcd.setCursor(13, 1);
    lcd.print(i);

  }
  set_Motor_4.run(RELEASE);
}
//-----------------------------------------------------
void Motor3Stop()
{

  digitalWrite(Sein1g, 0);
  digitalWrite(Sein1r, 1);
  digitalWrite(Sein2r, 1);
  digitalWrite(Sein2g, 0);

  Motor3_Stop();

  setBlok(1, 1);

  lcd.setCursor(13, 1);
  lcd.print("   ");
}

//-------------------------------------------------------
void Motor4Stop()
{

  digitalWrite(Sein1g, 1);
  digitalWrite(Sein1r, 0);
  digitalWrite(Sein2r, 0);
  digitalWrite(Sein2g, 1);

  Motor4_Stop();

  setBlok(1, 1);

  lcd.setCursor(13, 1);
  lcd.print("   ");
}
//---------------------------------------------------------------
void checkIngekomenSerial() {
  // Ga dan uit dat de serial commando's eruit zien als
  // [commando letter] [adres]
  // Waarbij ieder [] één byte is. Dus geen adressen in ASCII zenden ;)
  if (Serial.available() >= 2) {

    switch (Serial.read()) {

      case 'W':
        //Schakel een wissel recht
        //Ga even uit dat de functie schakeWissel(wisselnummer, richting) bestaat
        //schakelWissel(Serial.read(), RECHT);
        setWissel(1, 0);
        break;

      //schakel een wissel afbuigend
      case 'w':
        //schakelWissel(Serial.read(), AFBUIGEN);
        setWissel (1, 1);
        break;

      //Zet de richting van de motor
      case '.':

        digitalWrite(Sein1g, 1);
        digitalWrite(Sein1r, 0);
        digitalWrite(Sein2r, 1);
        digitalWrite(Sein2g, 0);

        setBlok(1, 0);

        Motor3_BACKWARD();
        break;

      case ',':

        digitalWrite(Sein1g, 0);
        digitalWrite(Sein1r, 1);
        digitalWrite(Sein2r, 0);
        digitalWrite(Sein2g, 1);

        setBlok(1, 0);

        Motor3_FORWARD();

        break;

      case 's':

        Motor3Stop();

        break;


    }
  }
}
//---------------------------------------------------------------
void setWissel(byte wisnr, byte wisstand)
{ // Begin subroutine setWissel

  if (wisnr == 1) {
    if (wisstand == 0) {

      lcd.setCursor(0, 1);
      lcd.print("  | Wissel 1 |  ");
    }
  }

  if (wisnr == 1) {
    if (wisstand == 1) {
      lcd.setCursor(0, 1);
      lcd.print("  / Wissel 1  /  ");
    }
  }
} // Einde sunroutine setWissel

void setBlok(byte blknr, byte blkstatus)
{ // Begin subroutine setBlok.

  if (blknr == 1) {
    if (blkstatus == 0) {

      lcd.setCursor(0, 0);
      lcd.print("  Blok 1 Vrij   ");

      digitalWrite(22, 0); // Blok 1 Vrij
      digitalWrite(23, 1); // Blok 2 Bezet
      digitalWrite(24, 0); // Blok 3 Vrij
      digitalWrite(25, 0); // Blok 4 Vrij
      digitalWrite(26, 0); // Blok 5 Vrij
      digitalWrite(27, 0); // Blok 6 Vrij
      digitalWrite(28, 0); // Blok 7 Vrij
      digitalWrite(29, 1); // Blok 8 Bezet
    }
  }
  if (blknr == 1) {
    if (blkstatus == 1) {

      lcd.setCursor(0, 0);
      lcd.print("  Blok 1 Bezet  ");

      digitalWrite(22, 1); // Blok 1 Bezet
      digitalWrite(23, 0); // Blok 2 Vrij
      digitalWrite(24, 0); // Blok 3 Vrij
      digitalWrite(25, 0); // Blok 4 Vrij
      digitalWrite(26, 0); // Blok 5 Vrij
      digitalWrite(27, 0); // Blok 6 Vrij
      digitalWrite(28, 0); // Blok 7 Vrij
      digitalWrite(29, 1); // Blok 8 Bezet

    }
  }
} // Einde routine setBlok