/* Sweep

  Programm for Servo-Cutter


*/

#include <Servo.h>
#include <JC_Button.h>
#include <EEPROM.h>

// create servo object to control a servo
Servo servo;

// Pin-settings (where to attach momentary pushbuttons to board)
byte PIN_SET_UPPER_POS = 7;
byte PIN_SET_LOWER_POS = 5;
byte PIN_CUT = 6;
byte PIN_SERVO = 9;
byte PIN_LED_OK = 12;
byte PIN_LED_ERR = 13;

// Attach servo signal wire to this pin:
byte INPUT_POT = A0;

// Button-definitions
int longPressTime = 1000;
int shortPressTime = 50;

Button SET_UPPER_POS(PIN_SET_UPPER_POS);
Button SET_LOWER_POS(PIN_SET_LOWER_POS);
Button CUT(PIN_CUT);

// Global working variables to store servo position
int pos = 0;    // variable to store the servo position
int pos_upper = 0;
int pos_lower = 0;

// State Machine
enum states_t {SET_MODE, SAFE_MODE, CUT_MODE, ERROR_MODE};
states_t STATE;

// Addresses to store positions in EEPROM
byte addr_pos_upper = 0;  // stored in EEPROM addr 0
byte addr_pos_lower = addr_pos_upper + sizeof(byte);  //addr 4
byte addr_INIT_done = 128;

void setup() {

  Serial.begin(9600);

  Serial.print("INIT done in EEPROM = ");
  Serial.println(EEPROM.read(addr_INIT_done));

  pinMode(PIN_LED_OK, OUTPUT);
  pinMode(PIN_LED_ERR, OUTPUT);

  if (EEPROM.read(addr_INIT_done) == 'X')
  {
    // Settings were already written once:
    // Read positions from EEPROM and
    // go directly to STATE CUT

    pos_upper = EEPROM.read(addr_pos_upper);
    pos_lower = EEPROM.read(addr_pos_lower);

    STATE = CUT_MODE;
  }

  else if (EEPROM.read(addr_INIT_done) != 'X')
  {
    // Program runs for the first time, so let's do a set up routine
    STATE = SET_MODE;
  }

  SET_UPPER_POS.begin();  // initialize the button object
  SET_LOWER_POS.begin();
  CUT.begin();

  servo.attach(PIN_SERVO);  // attaches the servo on pin 9 to the servo object

}

void loop() {

  // do this regularly to read the buttons
  SET_UPPER_POS.read();
  SET_LOWER_POS.read();
  CUT.read();

  switch (STATE)
  {
    case SET_MODE:
      {
        static bool pos_lower_set = 0;
        static bool pos_upper_set = 0;
        static byte pos_new = 0;

        pos = map(analogRead(INPUT_POT), 0, 1023, 0, 180);
        servo.write(pos);

        if (SET_LOWER_POS.wasReleased())
        {
          pos_lower = pos;
          pos_lower_set = 1;
          EEPROM.update(addr_pos_lower, pos_lower);
          Serial.print("Lower set to: ");
          Serial.println(pos_lower);

          // Blink once, delay is okay here, we're doing nothing else
          digitalWrite(PIN_LED_OK, HIGH);
          delay(150);
          digitalWrite(PIN_LED_OK, LOW);
        }

        if (SET_UPPER_POS.wasReleased())
        {
          pos_upper = pos;
          pos_upper_set = 1;
          EEPROM.update(addr_pos_upper, pos_upper);
          Serial.print("Upper set to: ");
          Serial.println(pos_upper);

          // Blink once, delay() is okay here, we're doing nothing else
          digitalWrite(PIN_LED_OK, HIGH);
          delay(150);
          digitalWrite(PIN_LED_OK, LOW);
        }

        if (CUT.wasPressed())
          // you can exit the setup ONLY if you've set both lower and upper position!
        {
          digitalWrite(PIN_LED_ERR, HIGH);
          delay(150);
          digitalWrite(PIN_LED_ERR, LOW);
        }

        if (CUT.pressedFor(longPressTime) && pos_lower_set == 1 && pos_upper_set == 1)
          // you can exit the setup ONLY if you've set both lower and upper position!
        {
          // setup done, write flag to EEPROM, so at next start setup will be skipped
          EEPROM.write(addr_INIT_done, 'X');

          Serial.print("INIT done in EEPROM = ");
          Serial.println(EEPROM.read(addr_INIT_done));

          // two quick flashes = setup done
          digitalWrite(PIN_LED_OK, HIGH);
          delay(150);
          digitalWrite(PIN_LED_OK, LOW);
          delay(150);
          digitalWrite(PIN_LED_OK, HIGH);
          delay(150);
          digitalWrite(PIN_LED_OK, LOW);

          STATE = SAFE_MODE;
        }
        break;
      }

    case SAFE_MODE:
      {

        if (CUT.wasReleased())
        {
          servo.write(pos_upper);
          STATE = CUT_MODE;
        }
        break;
      }


    case CUT_MODE:
      {

        if (CUT.wasReleased())
        {
          servo.write(pos_upper);
          //delay(250);
          servo.write(pos_lower);
          delay(500);
          servo.write(pos_upper);
        }
        break;
      }

    case ERROR_MODE:
      {
        servo.detach();
        digitalWrite(PIN_LED_ERR, HIGH);
        delay(100);
        digitalWrite(PIN_LED_ERR, LOW);
        delay(250);
      }
  }

}