#include <IRremote.hpp>
#include <EEPROM.h>
#include "EncoderStepCounter.h"

// Motor:
// OFF:   S1=H S2=L S3=H S4=L
// RIGHT: S1=L S2=L S3=H S4=H
// LEFT:  S1=H S2=H S3=L S4=L

#define BRIDGE_PIN_S1_PMOS 30
#define BRIDGE_PIN_S2_NMOS 32
#define BRIDGE_PIN_S3_PMOS 34
#define BRIDGE_PIN_S4_NMOS 36

#define PIN_RECEIVER 31
#define LED_PIN_UP 33
#define LED_PIN_DN 35

#define PIN_RC_PROG 37

#define BTN_VOL_UP 47
#define BTN_VOL_DN 46

// addresses in EEPROM
#define BTN_1_ADDR 11
#define BTN_2_ADDR 12
#define BTN_3_ADDR 13
#define BTN_4_ADDR 14
#define BTN_UP_ADDR 15
#define BTN_DN_ADDR 16
#define BTN_MUTE_ADDR 17
#define BTN_STBY_ADDR 18
#define BTN_VOL_UP_ADDR 19
#define BTN_VOL_DN_ADDR 20

#define DEFAULT_FLAG_ADDR 30 // EEPROM address to hold information for default RC codes recovery

#define DEFAULT_BTN_1_ADDR 31
#define DEFAULT_BTN_2_ADDR 32
#define DEFAULT_BTN_3_ADDR 33
#define DEFAULT_BTN_4_ADDR 34
#define DEFAULT_BTN_UP_ADDR 35
#define DEFAULT_BTN_DN_ADDR 36
#define DEFAULT_BTN_MUTE_ADDR 37
#define DEFAULT_BTN_STBY_ADDR 38
#define DEFAULT_BTN_VOL_UP_ADDR 39
#define DEFAULT_BTN_VOL_DN_ADDR 40

#define INIT_DEFAULT_FLAG_ADDR 29

#define ENCODER_PIN1 2 // 2 (32 = PD2) // CLK (A)
#define ENCODER_INT1 digitalPinToInterrupt(ENCODER_PIN1)
#define ENCODER_PIN2 3 // 3 (1 = PD3) // DT (B)
#define ENCODER_INT2 digitalPinToInterrupt(ENCODER_PIN2)

// Pins in comment defined for attiny88-au 32-pin
// Programming pins: 4-VCC, 5-GND, 29-Reset (PC6), 16-MISO (PB4), 15-MOSI (PB3), 17-SCK (PB5)
// Note: For 32pin Packages, PORTA exists. PA0 = A6/D23, PA1 = A7/D24, PA2 = D25, PA3 = D26
// First is Arduino pin (digital or analog), in bracked is Package pin
#define OUTPUT_1  22 // 9  (13 = PB1)
#define OUTPUT_2  23 // 10 (14 = PB2)
#define OUTPUT_3  24 // 11 (15 = PB3)
#define OUTPUT_4  25 // 12 (16 = PB4)
#define OUT_MUTE  26 // 8  (12 = PB0)
#define OUT_STBY  27 // 14 (7  = PB6)
#define OUT_ON    28 // 15 (8  = PB7)

#define BTN_1     40 // 4  (2 = PD4) 
#define BTN_2     41 // 1  (31 = PD1)
#define BTN_3     42 // 22 or A5 (28 = PC5)
#define BTN_4     43 // 21 or A4 (27 = PC4)
#define BTN_UP    44 // 20 or A3 (26 = PC3)
#define BTN_DN    45 // 19 or A2 (25 = PC2)
#define BTN_MUTE  53 // 25 (3  = PA2)
#define BTN_STBY  52 // 0  (30 = PD0) (E)

#define IN_REMOTE   51 // 23 or A6 (19 = PA0)

#define IN_DEFAULT  A0 // A0 (23 = PC0)

#define OUTPUT_ADDR 0 // OUTPUTs address in EEPROM

uint8_t outputSel = 0;

bool forceOnByRemote = false;
bool forceOffByRemote = false;
bool onByRemoteState = false;
bool mainState = false;
bool muteState = false;
int8_t btn_Stby_State = -1;
bool btnMute_pressed = false;

uint32_t oldTime_1ms = 0;

bool btnTimer_Active = false;
uint16_t btnTimer = 0;
bool btnStbyTimer_Active = false;
uint16_t btnStbyTimer = 0;
bool remoteStbyTimer_Active = false;
uint16_t remoteStbyTimer = 0;

bool rcBtnTimer_Active = false;
uint16_t rcBtnTimer = 0;
bool rcLedTimer_Active = false;
uint16_t rcLedTimer = 0;

//uint8_t prog_btn_pressed = 0;
uint8_t rc_code = 0;

int8_t motorState = 1;


// Create instance for one full step encoder
EncoderStepCounter encoder(ENCODER_PIN1, ENCODER_PIN2);
// Use the following for half step encoders
//EncoderStepCounter encoder(ENCODER_PIN1, ENCODER_PIN2, HALF_STEP);


void setup() {

  Serial.begin(9600);
  Serial.println("start");

  // Initialize encoder
  encoder.begin();
  // Initialize interrupts
  attachInterrupt(ENCODER_INT1, interrupt, CHANGE);
  attachInterrupt(ENCODER_INT2, interrupt, CHANGE);

  pinMode(OUTPUT_1, OUTPUT);
  pinMode(OUTPUT_2, OUTPUT);
  pinMode(OUTPUT_3, OUTPUT);
  pinMode(OUTPUT_4, OUTPUT);
  pinMode(OUT_MUTE, OUTPUT);
  pinMode(OUT_STBY, OUTPUT);
  pinMode(OUT_ON, OUTPUT);

  pinMode(BTN_1, INPUT_PULLUP);
  pinMode(BTN_2, INPUT_PULLUP);
  pinMode(BTN_3, INPUT_PULLUP);
  pinMode(BTN_4, INPUT_PULLUP);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DN, INPUT_PULLUP);
  pinMode(BTN_MUTE, INPUT_PULLUP);
  pinMode(BTN_STBY, INPUT_PULLUP);
  pinMode(IN_REMOTE, INPUT);
  pinMode(IN_DEFAULT, INPUT);

  pinMode(PIN_RC_PROG, INPUT_PULLUP);

  pinMode(BTN_VOL_UP, INPUT_PULLUP);
  pinMode(BTN_VOL_DN, INPUT_PULLUP);

  pinMode(LED_PIN_UP, OUTPUT);
  pinMode(LED_PIN_DN, OUTPUT);

  pinMode(BRIDGE_PIN_S1_PMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S2_NMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S3_PMOS, OUTPUT);
  pinMode(BRIDGE_PIN_S4_NMOS, OUTPUT);

  motor_off();

  IrReceiver.begin(PIN_RECEIVER, ENABLE_LED_FEEDBACK);
  
  outputZero();
  digitalWrite(OUT_MUTE, LOW); //MUTE
  digitalWrite(OUT_STBY, LOW); //StBy LED Red On (enabled by GND)
  digitalWrite(OUT_ON, HIGH); //StBy LED Blue Off (enabled by GND)

  mainState = false;
  onByRemoteState = false;

  delay(50);

  if (analogRead(IN_DEFAULT) < 100) { // Last Input is active when this pin is 0V
    if (EEPROM.read(OUTPUT_ADDR) > 10) {          // initialize empty EEPROM with value
      EEPROM.write(OUTPUT_ADDR, 1);
      delay(10);
    }
    outputSel = EEPROM.read(OUTPUT_ADDR);
    Serial.println(outputSel);
  }


  if (EEPROM.read(INIT_DEFAULT_FLAG_ADDR) != 1) {
    EEPROM.write(DEFAULT_BTN_1_ADDR, 48);
    EEPROM.write(DEFAULT_BTN_2_ADDR, 24);
    EEPROM.write(DEFAULT_BTN_3_ADDR, 122);
    EEPROM.write(DEFAULT_BTN_4_ADDR, 16);
    EEPROM.write(DEFAULT_BTN_UP_ADDR, 144);
    EEPROM.write(DEFAULT_BTN_DN_ADDR, 224);
    EEPROM.write(DEFAULT_BTN_MUTE_ADDR, 168);
    EEPROM.write(DEFAULT_BTN_STBY_ADDR, 162);
    EEPROM.write(DEFAULT_BTN_VOL_UP_ADDR, 2);
    EEPROM.write(DEFAULT_BTN_VOL_DN_ADDR, 152);
    EEPROM.write(INIT_DEFAULT_FLAG_ADDR, 1);
    Serial.println("default codes saved into eeprom - only once");
  }

  if ((EEPROM.read(DEFAULT_FLAG_ADDR) != 1) || ((digitalRead(BTN_1) == LOW) && (digitalRead(BTN_2) == LOW))) {
    EEPROM.write(BTN_1_ADDR, EEPROM.read(DEFAULT_BTN_1_ADDR));
    EEPROM.write(BTN_2_ADDR, EEPROM.read(DEFAULT_BTN_2_ADDR));
    EEPROM.write(BTN_3_ADDR, EEPROM.read(DEFAULT_BTN_3_ADDR));
    EEPROM.write(BTN_4_ADDR, EEPROM.read(DEFAULT_BTN_4_ADDR));
    EEPROM.write(BTN_UP_ADDR, EEPROM.read(DEFAULT_BTN_UP_ADDR));
    EEPROM.write(BTN_DN_ADDR, EEPROM.read(DEFAULT_BTN_DN_ADDR));
    EEPROM.write(BTN_MUTE_ADDR, EEPROM.read(DEFAULT_BTN_MUTE_ADDR));
    EEPROM.write(BTN_STBY_ADDR, EEPROM.read(DEFAULT_BTN_STBY_ADDR));
    EEPROM.write(BTN_VOL_UP_ADDR, EEPROM.read(DEFAULT_BTN_VOL_UP_ADDR));
    EEPROM.write(BTN_VOL_DN_ADDR, EEPROM.read(DEFAULT_BTN_VOL_DN_ADDR));
    EEPROM.write(DEFAULT_FLAG_ADDR, 1);
    digitalWrite(OUT_STBY, HIGH);
    delay(300);
    digitalWrite(OUT_STBY, LOW);
    delay(300);
    digitalWrite(OUT_STBY, HIGH);
    delay(300);
    digitalWrite(OUT_STBY, LOW);
    delay(300);
    digitalWrite(OUT_STBY, HIGH);
    delay(300);
    digitalWrite(OUT_STBY, LOW);
    delay(300);
    Serial.println("eeprom recovered");
  }

}


// Call tick on every change interrupt
void interrupt() {
  if (mainState == true) {
    encoder.tick();
  }
}


signed long position = 0;


void loop() {

  if (mainState == true) {
    signed char pos = encoder.getPosition();
    if (pos != 0) {
      if (pos > 0) {
        move_output(1);
      }
      if (pos < 0) {
        move_output(-1);
      }
      position += pos;
      encoder.reset();
    }
  }

  //Universal Timer
  if ((millis() - oldTime_1ms) >= 1) { // for 8 MHz delay is 2ms, for 16 MHz delay is 1ms
    oldTime_1ms = millis();
    if (btnTimer_Active) { btnTimer++; }
    if (btnStbyTimer_Active) {btnStbyTimer++; }
    // This complicated Timer is resistant to Remote fluctuations 
    // with help of Delay(1200) in part of code where "if (mainState == true) { onByRemoteState = true; }""
    if (remoteStbyTimer_Active) {
      if ((digitalRead(IN_REMOTE) == LOW) && (onByRemoteState != true)) {
        remoteStbyTimer++;
      } else if ((digitalRead(IN_REMOTE) == HIGH) && (mainState == true) && (onByRemoteState == true)) {
        remoteStbyTimer++;
      } else {
        remoteStbyTimer_Active = false;
        remoteStbyTimer = 0;
      }
    }
    if (rcBtnTimer_Active) { rcBtnTimer++; }
    if (rcLedTimer_Active) { rcLedTimer++; }
  }

  if (btnTimer > 250) { // for 8 MHz 1ms is 2ms (250 = 0.5s) ... for 16 MHz 1ms is 1ms (500 = 0.5s)
    btnTimer = 0;
    btnTimer_Active = false;
  }

  if (rcBtnTimer > 250) { // for 8 MHz 1ms is 2ms (250 = 0.5s) ... for 16 MHz 1ms is 1ms (500 = 0.5s)
    rcBtnTimer = 0;
    rcBtnTimer_Active = false;
  }

  if (rcLedTimer > 5000) { // for 8 MHz 1ms is 2ms (250 = 0.5s) ... for 16 MHz 1ms is 1ms (500 = 0.5s)
    rcLedTimer = 0;
    rcLedTimer_Active = false;
  }

  // Must be after Timers, but before main code to avoid conflicts
  if (digitalRead(PIN_RC_PROG) == LOW) {
    rc_programming();
    return;
  }

  check_received_ir_signal();

  // StandBy is toggled by pressing StandBy button in the lower part of code and activate btnStbyTimer
  // This part is detecting the activated timer, 0.5s for turn-Off, 0.15s for turn-On
  // For 8 MHz 1ms is 2ms (250 = 0.5s) ... for 16 MHz 1ms is 1ms (500 = 0.5s)
  if (((mainState == true) && (btnStbyTimer > 250)) || ((mainState == false) && (btnStbyTimer > 75))) {
    btnStbyTimer = 0;
    btnStbyTimer_Active = false;
    if (digitalRead(IN_REMOTE) == HIGH) {
      toggle_stby();
    }
  }

  if (remoteStbyTimer > 250) {
    Serial.println("Row 157, Remote Timer: ");
    Serial.println(remoteStbyTimer);
    remoteStbyTimer = 0;
    remoteStbyTimer_Active = false;
    if (onByRemoteState != true) {
      if(mainState != true) {
        forceOnByRemote = true;
        toggle_stby();
        forceOnByRemote = false;
      }
      onByRemoteState = true;
      delay(600);
    } else if (onByRemoteState == true) {
      forceOffByRemote = true;
      toggle_stby();
      forceOffByRemote = false;
      onByRemoteState = false;
      delay(600);
    }
  }


  if (mainState == true) {
    if ((digitalRead(BTN_1) == LOW) && (outputSel != 1) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      if (outputSel != 1) { set_output(1); }
    }
    if ((digitalRead(BTN_2) == LOW) && (outputSel != 2) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      if (outputSel != 2) { set_output(2); }
    }
    if ((digitalRead(BTN_3) == LOW) && (outputSel != 3) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      if (outputSel != 3) { set_output(3); }
    }
    if ((digitalRead(BTN_4) == LOW) && (outputSel != 4) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      if (outputSel != 4) { set_output(4); }
    }

    if ((digitalRead(BTN_UP) == LOW) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      move_output(1);
    }
    if ((digitalRead(BTN_DN) == LOW) && (btnTimer_Active != true)) {
      btnTimer_Active = true;
      move_output(-1);
    }

    if ((digitalRead(BTN_MUTE) == LOW) && (btnTimer_Active != true) && (btnMute_pressed != true)) {
      btnTimer_Active = true;
      btnMute_pressed = true;
      if (muteState != true) {
        digitalWrite(OUT_MUTE, HIGH);
        muteState = true;
      } else {
        digitalWrite(OUT_MUTE, LOW);
        muteState = false;
      }
    } else if ((digitalRead(BTN_MUTE) == HIGH) && (btnMute_pressed == true)) {
      btnMute_pressed = false;
    }
  }

  if ((digitalRead(BTN_STBY) == LOW) && (btnStbyTimer_Active != true) && (btn_Stby_State != 0)) {
    btn_Stby_State = 0;
    btnStbyTimer_Active = true;
  } else if ((digitalRead(BTN_STBY) == HIGH) && (btn_Stby_State != 1)) {
    btn_Stby_State = 1;
    btnStbyTimer_Active = false;
    btnStbyTimer = 0;
  }

  if ((digitalRead(IN_REMOTE) == LOW) && (onByRemoteState != true) && (remoteStbyTimer_Active != true)) {
    if (mainState != true) {
      remoteStbyTimer_Active = true;
      Serial.println("Remote LOW Detected, Timer true");
    }
    delay(1200);  // This delay holds onByRemoteState false in case of Remote fluctuations when device is active by Stby button
    if ((mainState == true) && (digitalRead(IN_REMOTE) == LOW)) { onByRemoteState = true; }
    delay(1000); // If delay is not present, toggle_stby() can be triggered unintentionally because IN_REMOTE can bounce HIGH
  } else if ((digitalRead(IN_REMOTE) == HIGH) && (onByRemoteState == true) && (remoteStbyTimer_Active != true)) {
    remoteStbyTimer_Active = true;
    remoteStbyTimer = 0;
  }

  if (mainState == true) {
    if ((digitalRead(BTN_VOL_UP) == LOW) && (digitalRead(BTN_VOL_DN) == HIGH)) {
      motor_up();
    } else if ((digitalRead(BTN_VOL_UP) == HIGH) && (digitalRead(BTN_VOL_DN) == LOW)) {
      motor_dn();
    } else {
      motor_off();
    }
  } else {
    motor_off();
  }
}


void toggle_stby() {
  if ((mainState == true) || (forceOffByRemote == true)) {
    digitalWrite(OUT_MUTE, HIGH);
    delay(200);
    outputZero();
    outputSel = 0;
    delay(100);
    digitalWrite(OUT_MUTE, LOW);
    digitalWrite(OUT_STBY, LOW); //StBy LED Red On
    digitalWrite(OUT_ON, HIGH); //StBy LED Blue Off
    mainState = false;
  } else if ((mainState != true) || (forceOnByRemote == true)) {
    digitalWrite(OUT_MUTE, HIGH);
    digitalWrite(OUT_STBY, HIGH); //StBy LED Red Off
    digitalWrite(OUT_ON, LOW); //StBy LED Blue On
    delay(600);
    if(analogRead(IN_DEFAULT) < 100) { // Last Input is active
      outputSel = EEPROM.read(OUTPUT_ADDR);
      set_output(outputSel);
      Serial.println("last output: ");
      Serial.println(outputSel);
    } else {
      outputSel = default_inputVoltage_to_outputPin(analogRead(IN_DEFAULT));
      Serial.println(analogRead(IN_DEFAULT));
      Serial.println(outputSel);
      set_output(outputSel);
    }
    delay(200);
    digitalWrite(OUT_MUTE, LOW);
    mainState = true;
  }
}


void move_output(int8_t dir) {
  switch(dir) {
    case 1:
      outputSel++;
      if (outputSel > 4) { outputSel = 1; }
      set_output(outputSel);
      break;
    case -1:
      outputSel--;
      if (outputSel < 1) { outputSel = 4; }
      set_output(outputSel);
      break;
  }
}


void set_output(uint8_t sel) {
    if (muteState != true) { digitalWrite(OUT_MUTE, HIGH); }
    delay(200);
    outputZero();
    delay(100);
    switch(sel) {
      case 1:
        digitalWrite(OUTPUT_1, HIGH);
        break;
      case 2:
        digitalWrite(OUTPUT_2, HIGH);
        break;
      case 3:
        digitalWrite(OUTPUT_3, HIGH);
        break;
      case 4:
        digitalWrite(OUTPUT_4, HIGH);
        break;
      default:
        break;
    }
    outputSel = sel;
    EEPROM.write(OUTPUT_ADDR, sel);
    delay(10);
    Serial.println(EEPROM.read(OUTPUT_ADDR));
    delay(200);
    if (muteState != true) { digitalWrite(OUT_MUTE, LOW); }
}


uint16_t default_inputVoltage_to_outputPin(uint16_t voltage) {
  Serial.println("voltage in function");
  Serial.println(voltage);
  if ((voltage > 128) && (voltage < 384)) { return 1; }
  else if ((voltage > 384) && (voltage < 640)) { return 2; }
  else if ((voltage > 640) && (voltage < 896)) { return 3; }
  else if (voltage > 896) { return 4; }
  else { return 1; }
}


void outputZero() {
  digitalWrite(OUTPUT_1, LOW);
  digitalWrite(OUTPUT_2, LOW);
  digitalWrite(OUTPUT_3, LOW);
  digitalWrite(OUTPUT_4, LOW);
}


void check_received_ir_signal() {
  // Checks received an IR signal
  if (IrReceiver.decode()) {
    if ((IrReceiver.decodedIRData.flags & IRDATA_FLAGS_IS_REPEAT)) {
      if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_UP_ADDR)) {
        rcBtnTimer_Active = true;
        rcBtnTimer = 0;
        motor_up();
      }
      if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_DN_ADDR)) {
        rcBtnTimer_Active = true;
        rcBtnTimer = 0;
        motor_dn();
      }
      IrReceiver.restartAfterSend();
      IrReceiver.resume();  // Receive the next value
      return;
    } else {
        if (mainState == true) {
          if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_1_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (outputSel != 1) { set_output(1); }
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_2_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (outputSel != 2) { set_output(2); }
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_3_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (outputSel != 3) { set_output(3); }
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_4_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (outputSel != 4) { set_output(4); }
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_UP_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            move_output(1);
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_DN_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            move_output(-1);
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_MUTE_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (muteState != true) {
              digitalWrite(OUT_MUTE, HIGH);
              muteState = true;
            } else {
              digitalWrite(OUT_MUTE, LOW);
              muteState = false;
            }
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_UP_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            motor_up();
          } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_DN_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            motor_dn();
          }
        }
        if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_STBY_ADDR)) {
            rcBtnTimer_Active = true;
            rcBtnTimer = 0;
            if (digitalRead(IN_REMOTE) == HIGH) {
              toggle_stby();
            }
        }
    }
    IrReceiver.resume();
  } 
  if (rcBtnTimer_Active != true) {
    if ((motorState != 0) && (digitalRead(BTN_VOL_UP) == HIGH) && (digitalRead(BTN_VOL_DN) == HIGH)) {
      motor_off();
    }
  }
}



/*
void check_received_ir_signal() {
  // Checks received an IR signal
  if (IrReceiver.decode()) {
//    unsigned long keycode = IrReceiver.decodedIRData.command;
    Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX);
    Serial.println("one click");
    IrReceiver.printIRResultShort(&Serial);
    IrReceiver.printIRSendUsage(&Serial);
    if ((IrReceiver.decodedIRData.flags & IRDATA_FLAGS_IS_REPEAT)) {
      Serial.println("repeat");
      if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_UP_ADDR)) {
        rcBtnTimer_Active = true;
        rcBtnTimer = 0;
        digitalWrite(LED_PIN_UP, HIGH);
        motor_up();
      }
      if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_DN_ADDR)) {
        rcBtnTimer_Active = true;
        rcBtnTimer = 0;
        digitalWrite(LED_PIN_DN, HIGH);
        motor_dn();
      }
      IrReceiver.restartAfterSend();
      IrReceiver.resume();  // Receive the next value
      return;
    } else {
        if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_1_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          if (outputSel != 1) { set_output(1); }
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_2_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          if (outputSel != 2) { set_output(2); }
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_3_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          if (outputSel != 3) { set_output(3); }
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_4_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          if (outputSel != 4) { set_output(4); }
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_UP_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          move_output(1);
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_DN_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          move_output(-1);
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_MUTE_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          if (muteState != true) {
            digitalWrite(OUT_MUTE, HIGH);
            muteState = true;
          } else {
            digitalWrite(OUT_MUTE, LOW);
            muteState = false;
          }
        } else if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_STBY_ADDR)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          Serial.println("StandBy press");
          if (digitalRead(IN_REMOTE) == HIGH) {
            toggle_stby();
          }
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_UP_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          motor_up();
          digitalWrite(LED_PIN_UP, HIGH);
          Serial.println("VolUp press");
        } else if ((IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_DN_ADDR)) && (mainState == true)) {
          rcBtnTimer_Active = true;
          rcBtnTimer = 0;
          motor_dn();
          digitalWrite(LED_PIN_DN, HIGH);
          Serial.println("VolDn press");
        }
    }
    IrReceiver.resume();
  } 
  if (rcBtnTimer_Active != true) {
    if ((digitalRead(LED_PIN_UP) == HIGH) || (digitalRead(LED_PIN_DN) == HIGH)) {
      digitalWrite(LED_PIN_UP, LOW);
      digitalWrite(LED_PIN_DN, LOW);
    }
    if ((motorState != 0) && (digitalRead(BTN_VOL_UP) == HIGH) && (digitalRead(BTN_VOL_DN) == HIGH)) {
      motor_off();
    }
  }
}
*/


void rc_programming() {
  if (rc_code == 0) {
    read_rc_code();
  } else {
    blink_led_rc_prog(true);
    if (digitalRead(BTN_1) == LOW) {
      program_button_to_eeprom(BTN_1_ADDR);
    }
    if (digitalRead(BTN_2) == LOW) {
      program_button_to_eeprom(BTN_2_ADDR);
    }
    if (digitalRead(BTN_3) == LOW) {
      program_button_to_eeprom(BTN_3_ADDR);
    }
    if (digitalRead(BTN_4) == LOW) {
      program_button_to_eeprom(BTN_4_ADDR);
    }
    if (digitalRead(BTN_UP) == LOW) {
      program_button_to_eeprom(BTN_UP_ADDR);
    }
    if (digitalRead(BTN_DN) == LOW) {
      program_button_to_eeprom(BTN_DN_ADDR);
    }
    if (digitalRead(BTN_MUTE) == LOW) {
      program_button_to_eeprom(BTN_1_ADDR);
    }
    if (digitalRead(BTN_STBY) == LOW) {
      program_button_to_eeprom(BTN_1_ADDR);
    }
    if (digitalRead(BTN_VOL_UP) == LOW) {
      program_button_to_eeprom(BTN_VOL_UP_ADDR);
    }
    if (digitalRead(BTN_VOL_DN) == LOW) {
      program_button_to_eeprom(BTN_VOL_DN_ADDR);
    }
  }
}

void program_button_to_eeprom(uint8_t eepromBtnAddress) {
  if (rc_code != EEPROM.read(eepromBtnAddress)) {
    EEPROM.write(eepromBtnAddress, rc_code);
    delay(30);
  }
  Serial.println("BTN_x programmed: ");
  Serial.print(rc_code);
  Serial.println(" ");
  rc_code = 0;
//  prog_btn_pressed = 0;
  blink_led_rc_prog(false);
  //        digitalWrite(LED_PIN_RC_PROG, HIGH);
  digitalWrite(OUT_STBY, HIGH);
}

void read_rc_code() {
  if (IrReceiver.decode()) {
    rc_code = IrReceiver.decodedIRData.command;
    Serial.println(IrReceiver.decodedIRData.command);
    Serial.println(rc_code);
    Serial.println("rc_code ok");
    Serial.println(" ");
    IrReceiver.resume();
  } 
}


void blink_led_rc_prog(bool rcLedBlink) {
  if (rcLedBlink == true) { rcLedTimer_Active = true; }
  if ((rcLedBlink == true) && (rcLedTimer > 250)) {
    digitalWrite(OUT_STBY, !digitalRead(OUT_STBY));
    rcLedTimer = 0;
  }
  if (rcLedBlink == false) {
    rcLedTimer_Active = false;
    rcLedTimer = 0;
  }
}

/*
void control_volume_RC() {
  // Takes command based on IR code received
  if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_UP_ADDR)) {
    motor_up();
  } else {
    motor_off();
  }
  if (IrReceiver.decodedIRData.command == EEPROM.read(BTN_VOL_DN_ADDR)) {
    motor_dn();
  } else {
    motor_off();
  }
}
*/

// Motor OFF
void motor_off() {
  if (motorState != 0) {
    digitalWrite(BRIDGE_PIN_S1_PMOS, HIGH);
    digitalWrite(BRIDGE_PIN_S2_NMOS, LOW);
    digitalWrite(BRIDGE_PIN_S3_PMOS, HIGH);
    digitalWrite(BRIDGE_PIN_S4_NMOS, LOW);
    delay(30);
    motorState = 0;
  }
}

// Motor UP
void motor_up() {
  if (motorState != 1) {
    // first off pins
    digitalWrite(BRIDGE_PIN_S2_NMOS, LOW);
    digitalWrite(BRIDGE_PIN_S3_PMOS, HIGH);
    delay(300); // 300 is only for testing visibility, decrease to 30 for real
    // then on pins
    digitalWrite(BRIDGE_PIN_S1_PMOS, LOW);
    digitalWrite(BRIDGE_PIN_S4_NMOS, HIGH);
    delay(300);
    motorState = 1;
  }
}

// Motor DOWN
void motor_dn() {
  if (motorState != -1) {
    // first off pins
    digitalWrite(BRIDGE_PIN_S1_PMOS, HIGH);
    digitalWrite(BRIDGE_PIN_S4_NMOS, LOW);
    delay(300);
    // then on pins
    digitalWrite(BRIDGE_PIN_S2_NMOS, HIGH);
    digitalWrite(BRIDGE_PIN_S3_PMOS, LOW);
    delay(300);
    motorState = -1;
  }
}