#define DEBUG
#define PRINTOUT

#include <Servo.h>

enum HARDWARE { //Defining the pinouts for the Teensy 3.2

  //RS485 pinouts
  RS485_EN = 2,
  RS485_RX = 7,
  RS485_TX = 8,

  //Motor controls
  CMOTO_CURRENT = 0,
  CMOTO_PWM = 9,
  CMOTO_DA = 11,
  CMOTO_DB = 12,
  CMOTO_SEL = 13,

  //Servo controls
  SERVO1 = 25,
  SERVO2 = 22,
  SERVO3 = 23,
  SERVO4 = 21,
  SERVO5 = 20,

  //MOSFET Motor Systems
  MOTO1 = 4,
  MOTO2 = 3,

  //MOSFET Solenoid Systems
  SOL1 = 5, //currently non operational
  SOL2 = 10,

  //Limit Switches
  LS1 = 17,
  LS2 = 18,
  LS3 = 19,

  //LED Light Ring
  LED_RING = 24,

  //RGB LED
  LED_RED = 1,
  LED_GREEN = 32,
  LED_BLUE = 6,

  //Supplementry Pins (Currently Unused)
  P_15 = 15,
  P_16 = 16

};

//Class Instantiations
Servo servo1, servo2, servo3, servo4, servo5;

//Global Variables
int pwm_speed = 0, multi = 0, multi2 = 0;
bool dir = false, limit1, limit2, limit3;

void setup() {

  Serial.begin(9600);

  set_pinmodes(); //performs all the pin setup
  set_basecase(); //sets the base pin states

}

void loop() {

  motor_control();
  limit_switch_detect();

#ifdef DEBUG
  debug();
#endif

#ifdef PRINTOUT
  printout();
#endif

}

void set_pinmodes() {

  //RS485 Transciever
  pinMode(HARDWARE::RS485_EN, OUTPUT);
  pinMode(HARDWARE::RS485_RX, INPUT);
  pinMode(HARDWARE::RS485_TX, INPUT);

  //Controllable Motor Controller
  pinMode(HARDWARE::CMOTO_CURRENT, INPUT);
  pinMode(HARDWARE::CMOTO_PWM, OUTPUT);
  pinMode(HARDWARE::CMOTO_DA, OUTPUT);
  pinMode(HARDWARE::CMOTO_DB, OUTPUT);
  pinMode(HARDWARE::CMOTO_SEL, OUTPUT);

  //Servo Control Pins
  pinMode(HARDWARE::SERVO1, OUTPUT);
  pinMode(HARDWARE::SERVO2, OUTPUT);
  pinMode(HARDWARE::SERVO3, OUTPUT);
  pinMode(HARDWARE::SERVO4, OUTPUT);
  pinMode(HARDWARE::SERVO5, OUTPUT);

  //MOSFET Motor Control
  pinMode(HARDWARE::MOTO1, OUTPUT);
  pinMode(HARDWARE::MOTO2, OUTPUT);

  //MOSFET Solenoid Control
  pinMode(HARDWARE::SOL1, OUTPUT);
  pinMode(HARDWARE::SOL2, OUTPUT);

  //Limit Switch Detection
  pinMode(HARDWARE::LS1, INPUT);
  pinMode(HARDWARE::LS2, INPUT);
  pinMode(HARDWARE::LS3, INPUT);

  //LED Light Ring
  pinMode(HARDWARE::LED_RING, OUTPUT);

  //RGB LED Setup
  pinMode(HARDWARE::LED_RED, OUTPUT);
  pinMode(HARDWARE::LED_GREEN, OUTPUT);
  pinMode(HARDWARE::LED_BLUE, OUTPUT);

}

void set_basecase() {

  //Set the RGB to base case of RED
  digitalWrite(HARDWARE::LED_RED, LOW);
  digitalWrite(HARDWARE::LED_GREEN, HIGH);
  digitalWrite(HARDWARE::LED_BLUE, HIGH);

  //Setting controlled motor to have a PWM frequency out of audible range
  analogWriteFrequency(HARDWARE::CMOTO_PWM, 25000);
  analogWriteFrequency(HARDWARE::MOTO1, 25000);

  //Attach servo classes to servo pins
  servo1.attach(HARDWARE::SERVO1);
  servo2.attach(HARDWARE::SERVO2);
  servo3.attach(HARDWARE::SERVO3);
  servo4.attach(HARDWARE::SERVO4);
  servo5.attach(HARDWARE::SERVO5); //continous rotation servo

  //Setting MOSFET controlled motors to be off
  digitalWrite(HARDWARE::MOTO1, LOW);
  digitalWrite(HARDWARE::MOTO2, LOW);

  //Setting MOSFET controlled solenoids to be off
  digitalWrite(HARDWARE::SOL1, LOW);
  digitalWrite(HARDWARE::SOL2, LOW);

  //Setting the LED ring light to be off
  digitalWrite(HARDWARE::LED_RING, LOW);

}

//Sets motor direction, nothing fancy
void motor_control() {

  analogWrite(HARDWARE::CMOTO_PWM, pwm_speed);
  digitalWrite(HARDWARE::CMOTO_DA, dir);
  digitalWrite(HARDWARE::CMOTO_DB, !dir);
  digitalWrite(HARDWARE::CMOTO_SEL, dir);

}

//Limit switch detection with some debug that involves the RGB LED
void limit_switch_detect() {

  limit1 = digitalRead(HARDWARE::LS1);
  limit2 = digitalRead(HARDWARE::LS2);
  limit3 = digitalRead(HARDWARE::LS3);

#ifdef DEBUG
  if (limit1 || limit2 || limit3) digitalWrite(HARDWARE::LED_BLUE, LOW);
  else digitalWrite(HARDWARE::LED_BLUE, HIGH);
#endif

}


#ifdef DEBUG

//for full use with an open serial monitor
void debug() {

  if (Serial.available()) {

    char choice = Serial.read();

    if (choice == 'a') { //command: a [val], sets servo 1 to angle of val
      int val = Serial.parseInt();
      servo1.write(val);
      multi = val;
    }

    else if (choice == 'b') { //command: b [val], sets servo 2 to angle of val
      int val = Serial.parseInt();
      servo2.write(val);
      multi = val;
    }

    else if (choice == 'c') { //command: c [val], sets servo 3 to angle of val
      int val = Serial.parseInt();
      servo3.write(val);
      multi = val;
    }

    else if (choice == 'd') { //command: d [val], sets servo 4 to angle of val
      int val = Serial.parseInt();
      servo4.write(val);
      multi = val;
    }

    else if (choice == 'e') { //command: e [val], sets servo 5 direction speed (0 full one dir, 90 none, 180 full other dir)
      int val = Serial.parseInt();
      servo5.write(val);
      multi = val;
    }

    else if (choice == 'm') { //command: m [pwm_val] [dir], sets speed and direction
      pwm_speed = Serial.parseInt();
      dir = Serial.parseInt();
    }

    else if (choice == 'n') { //command: n [1/0], turns the motor 1 on or off
      int on = Serial.parseInt();
    }

    else if (choice == 'o') { //command: o [1/0], turns the motor 2 on or off
      int on = Serial.parseInt();
      multi = on;
      delay(1000);
      if (on >= 1) {
        multi2 = on;
        for (int i = 0; i <= on; i++) { //ramping down the current so it's not instant on
          analogWrite(HARDWARE::MOTO1, i);
          Serial.println(i);
          delay(10);
        }
      }
      else if (on == 0) {
        for (int i = multi2; i >= 0; i--) { //ramping down the current so it's not instant on
          analogWrite(HARDWARE::MOTO1, i);
          Serial.println(i);
          delay(10);
        }
      }
    }

    else if (choice == 's') { //command: s [1/0], turns the solenoid on or off
      int on = Serial.parseInt();
      if (on == 1) digitalWrite(HARDWARE::SOL1, HIGH);
      else digitalWrite(HARDWARE::SOL1, LOW);
    }

    else if (choice == 't') { //command: s [1/0], turns the solenoid on or off
      int on = Serial.parseInt();
      if (on == 1) digitalWrite(HARDWARE::SOL2, HIGH);
      else digitalWrite(HARDWARE::SOL2, LOW);
    }

    else if (choice == 'x') { //command: x [1/0], turns the LED ring light on or off
      int on = Serial.parseInt();
      if (on == 1) digitalWrite(HARDWARE::LED_RING, HIGH);
      else digitalWrite(HARDWARE::LED_RING, LOW);
    }

    else if (choice == 'z') {
      int on = Serial.parseInt();
      if (on == 1) {
        digitalWrite(HARDWARE::LED_RED, HIGH);
        digitalWrite(HARDWARE::LED_GREEN, LOW);
        digitalWrite(HARDWARE::LED_BLUE, HIGH);
      }
      else {
        digitalWrite(HARDWARE::LED_RED, LOW);
        digitalWrite(HARDWARE::LED_GREEN, HIGH);
        digitalWrite(HARDWARE::LED_BLUE, HIGH);
      }
    }

    while (Serial.available()) Serial.read();

  }
}

#endif

#ifdef PRINTOUT

//un-labeled printouts, but printouts nonetheless
void printout() {

  Serial.print(pwm_speed);
  Serial.print(",   ");
  Serial.print(dir);
  Serial.print(",   ");
  Serial.print(limit1);
  Serial.print(",   ");
  Serial.print(limit2);
  Serial.print(",   ");
  Serial.print(limit3);
  Serial.print(",   ");
  Serial.println(multi);

}

#endif