/*
  "IOT-OWL" implementation based on the paper
  https://research.ulapland.fi/en/publications/iot-owl-soft-tangible-user-interface-for-detecting-the-presence-o
  by Jasper Honkasalo.

  This implementation uses a basic state-machine-based approach to switch
  between the three defined states.

  HSV <-> RGB color conversion is used to cycle the color spectrum.

  NOTE: WokWi uses an old version of Servo.h, requiring servos to be attached on pin 9 and/or 10.
  More info: https://www.arduino.cc/reference/en/libraries/servo/attach/ .
  This may also cause slight flickering in the RGB-LED.
*/

/* ----- INCLUDES ----- */

#include <Servo.h>

/* ----- DEFINES ----- */

#define LED_R_PIN 13  // LED red pin
#define LED_G_PIN 12  // LED green pin
#define LED_B_PIN 11  // LED blue pin
#define LED_MOOD_R_PIN 6  // Mood "Smart Textile" LED red pin
#define LED_MOOD_G_PIN 5  // Mood "Smart Textile" LED green pin
#define LED_MOOD_B_PIN 3  // Mood "Smart Textile" LED blue pin
#define BTN_R_PIN 7   // Crowded state
#define BTN_G_PIN 4   // Space ("not crowded") state
#define BTN_B_PIN 2   // Friend state
#define BTN_Y_PIN 8   // Friend state reset switch
#define SRV_R_PIN 10   // Right servo
#define SRV_L_PIN 9   // Left servo
#define WING_OPEN_ANGLE 90    // Servo degrees for when the wings are in "open" state
#define WING_CLOSED_ANGLE 0   // Same as above but for "closed" state
#define WING_FLAP_ANGLE 45    // The total angle the wings will "flap" when in FRIEND state

/* ----- STATE DEFINITIONS ----- */

// Enumeration of all the possible states
typedef enum {
    CROWDED,
    SPACE,
    FRIEND
} State;

/* ----- RUNTIME STATE MANAGEMENT ----- */

// Servo references
Servo servoR;
Servo servoL;

// Target positions for servos, in degrees
int servoRPos = WING_CLOSED_ANGLE;
int servoLPos = WING_CLOSED_ANGLE;

// Flag for flip-flopping the target angle of wing servos, when in FRIEND state
bool flapFlipFlop = false;

// Flag for stopping the wing-flapping when in FRIEND state.
bool isFriendStateReset = false;

// Initial state of the state-machine
State currentState;

// "Smart Textile" coloring
float colorBuffer[3];         // Intermediary buffer used to convert between HSV <-> RGB
float hue = 0.0;              // The current HUE of the LED
float hueCycleSpeed = 0.001f;  // How fast to cycle the HUE.

/* ----- LOGIC ----- */

void setup() {
  // Setup pinmodes
  pinMode(LED_R_PIN, OUTPUT);
  pinMode(LED_G_PIN, OUTPUT);
  pinMode(LED_B_PIN, OUTPUT);
  pinMode(LED_MOOD_R_PIN, OUTPUT);
  pinMode(LED_MOOD_G_PIN, OUTPUT);
  pinMode(LED_MOOD_B_PIN, OUTPUT);
  pinMode(BTN_R_PIN, INPUT_PULLUP);
  pinMode(BTN_G_PIN, INPUT_PULLUP);
  pinMode(BTN_B_PIN, INPUT_PULLUP);
  pinMode(BTN_Y_PIN, INPUT_PULLUP);

  // Setup servos. Please read the NOTE section in the file header
  servoR.attach(10);
  servoL.attach(9);

  // Transition to the initial state
  transitionToState(CROWDED);
}

void loop() {
  // State machine logic
  onExecuteState(currentState);
  
  // Move servos towards their target positions
  servoR.write(servoRPos);
  servoL.write(servoLPos);

  // Update the "Smart Textile"
  updateMoodColors();

  // Limit the update speed, to not flood servos with position updates
  delay(15);
}

void updateMoodColors() {
  // Convert the current hue to RGB color, and write to LED
  setColor(hsv2rgb(hue, 1.0, 1.0, colorBuffer));

  // Increase the hue
  hue += hueCycleSpeed;

  // Ensure hue doesn't go out of bounds
  if (hue >= 1.0) hue = 0.0;
}

void setColor(float *rgb) {
  // AnalogWrite expects a byte value. If we were to use a CA-diode, the rgb should be inverted (1 - rgb[0]).
  analogWrite(LED_MOOD_R_PIN, (int)(rgb[0] * 255));
  analogWrite(LED_MOOD_G_PIN, (int)(rgb[1] * 255));
  analogWrite(LED_MOOD_B_PIN, (int)(rgb[2] * 255));
}

void transitionToState(State newState) {
    onExitState(currentState);
    onEnterState(newState);
    currentState = newState;
}

void onEnterState(State newState) {
  switch (newState) {
    case CROWDED:
      // Set eye color to red
      digitalWrite(LED_R_PIN, HIGH);
      // Set wings to "closed" angle
      servoRPos = WING_CLOSED_ANGLE;
      servoLPos = WING_CLOSED_ANGLE;
      break;

    case SPACE:
      // Set eye color to green
      digitalWrite(LED_G_PIN, HIGH);
      // Set wings to "open" angle
      servoRPos = WING_OPEN_ANGLE;
      servoLPos = WING_OPEN_ANGLE;
      break;

    case FRIEND:
      // Set eye color to blue
      digitalWrite(LED_B_PIN, HIGH);
      // Set wings to "closed" angle
      servoRPos = WING_CLOSED_ANGLE;
      servoLPos = WING_CLOSED_ANGLE;
      isFriendStateReset = false;
      break;
  }
}

void onExecuteState(State state) {
  switch (state) {
    case CROWDED:
      if (digitalRead(BTN_G_PIN) == LOW)
        transitionToState(SPACE);
      else if (digitalRead(BTN_B_PIN) == LOW)
        transitionToState(FRIEND);
      break;

    case SPACE:
      if (digitalRead(BTN_R_PIN) == LOW)
        transitionToState(CROWDED);
      else if (digitalRead(BTN_B_PIN) == LOW)
        transitionToState(FRIEND);
      break;

    case FRIEND:
      if (digitalRead(BTN_R_PIN) == LOW)
        transitionToState(CROWDED);
      else if (digitalRead(BTN_G_PIN) == LOW)
        transitionToState(SPACE);
      else if (digitalRead(BTN_Y_PIN) == LOW)
        isFriendStateReset = true;

      // Skip flapping the wings, if the user has "reset" the FRIEND state
      if (isFriendStateReset) {
        servoRPos = WING_CLOSED_ANGLE;
        servoLPos = WING_CLOSED_ANGLE;
        break;
      }

      // Depending on the flip-flop, either increase or decrease the servo target angle
      if (flapFlipFlop) {
        servoRPos++;
        servoLPos++;
      }
      else {
        servoRPos--;
        servoLPos--;
      }

      // When in target rotation, flip the flip-flop to change direction
      if (abs(servoRPos) >= WING_FLAP_ANGLE)
        flapFlipFlop = !flapFlipFlop;
      break;
  }
}

void onExitState(State previousState) {
  // Reset LED states
  digitalWrite(LED_R_PIN, LOW);
  digitalWrite(LED_G_PIN, LOW);
  digitalWrite(LED_B_PIN, LOW);
}

/* ----- HSV -> RGB CONVERSION ----- */
// Expects HSV channels to be defined in [0.0 .. 1.0] interval
float fract(float x) { return x - int(x); }
float mix(float a, float b, float t) { return a + (b - a) * t; }
float step(float e, float x) { return x < e ? 0.0 : 1.0; }

float* hsv2rgb(float h, float s, float b, float* rgb) {
  rgb[0] = b * mix(1.0, constrain(abs(fract(h + 1.0) * 6.0 - 3.0) - 1.0, 0.0, 1.0), s);
  rgb[1] = b * mix(1.0, constrain(abs(fract(h + 0.6666666) * 6.0 - 3.0) - 1.0, 0.0, 1.0), s);
  rgb[2] = b * mix(1.0, constrain(abs(fract(h + 0.3333333) * 6.0 - 3.0) - 1.0, 0.0, 1.0), s);
  return rgb;
}