#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include "clockface.h"
//#include "bitmaps.h"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int eyeWidth = 16;
const int eyeHeight = 16;

Servo leftArmServo;
Servo rightArmServo;

const int ARM_SERVO_LEFT_PIN = 6; // Adjust pin numbers as per your setup
const int ARM_SERVO_RIGHT_PIN = 7;

const int TRIGGER_PIN = 12;
const int ECHO_PIN = 11;
const int BUZZER_PIN = 8;
const int LEFT_MOTOR_FORWARD_PIN = 5;
const int LEFT_MOTOR_BACKWARD_PIN = 6;
const int RIGHT_MOTOR_FORWARD_PIN = 7;
const int RIGHT_MOTOR_BACKWARD_PIN = 8;
const int MORSE_INPUT_PIN = 9; // Pin where Morse code input is connected

struct MorseMapping {
  char character;
  const char* code;
};

const MorseMapping morseMappings[] = {
  {'A', ".-"},
  {'B', "-..."},
  {'C', "-.-."},
  {'D', "-.."},
  {'E', "."},
  {'F', "..-."},
  {'G', "--."},
  {'H', "...."},
  {'I', ".."},
  {'J', ".---"},
  {'K', "-.-"},
  {'L', ".-.."},
  {'M', "--"},
  {'N', "-."},
  {'O', "---"},
  {'P', ".--."},
  {'Q', "--.-"},
  {'R', ".-."},
  {'S', "..."},
  {'T', "-"},
  {'U', "..-"},
  {'V', "...-"},
  {'W', ".--"},
  {'X', "-..-"},
  {'Y', "-.--"},
  {'Z', "--.."}
};

const int numMappings = sizeof(morseMappings) / sizeof(morseMappings[0]);



enum Emotion {
  HAPPY,
  SAD,
  ANGRY,
  NEUTRAL
};

void avoidObstacle();
unsigned int readUltrasonicDistance();
void moveForward();
void moveBackward();
void stopMovement();
void expressEmotion(Emotion emotion);
void displayEmotionBitmap(Emotion emotion);
void reactToCommand(String command);
void sayYes();
void sayNo();
String readSerialInput();
void updateEyes();
unsigned char readkey(void);
String readMorseInput();
void morseCodeInterpreter(char character);
void moveArms(Servo& leftServo, Servo& rightServo, int angle);

void setup() {
  //Wire.begin();
  Serial.begin(115200);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Loop forever if initialization fails
  }

  display.display();
  delay(2000); // Pause for 2 seconds
  display.clearDisplay();


  // Initialize arm servos
  leftArmServo.attach(ARM_SERVO_LEFT_PIN);
  rightArmServo.attach(ARM_SERVO_RIGHT_PIN);

  // Initialize pins for motors, ultrasonic sensor, and eye control
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LEFT_MOTOR_FORWARD_PIN, OUTPUT);
  pinMode(LEFT_MOTOR_BACKWARD_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_FORWARD_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_BACKWARD_PIN, OUTPUT);
  pinMode(MORSE_INPUT_PIN, INPUT);

  Serial.println(F("Setup completed"));
}

void loop() {
  avoidObstacle();

  unsigned int distance = readUltrasonicDistance();
  Serial.print(F("Distance: "));
  Serial.print(distance);
  Serial.println(F("cm"));

  String command = readSerialInput();
  if (command.length() > 0) {
    reactToCommand(command);
  }

  String morseCode = readMorseInput();
  if (morseCode.length() > 0) {
    for (int i = 0; i < numMappings; ++i) {
      if (strcmp(morseMappings[i].code, morseCode.c_str()) == 0) {
        morseCodeInterpreter(morseMappings[i].character);
        break;
      }
    }
  }

  updateEyes();

  delay(100); // Adjust delay as needed for response time
}

void updateEyes() {
  static int xp = 16;
  static int mood = 1;
  static int xd = 0, espera = 0, step = 0;

  if (step == 0) {
    display.clearDisplay();
    displayEmotionBitmap(HAPPY);   // Initial state (example: happy)
    display.display();
    delay(2000);
    step = 1;
  } else {
    espera++;
    if (espera == 3) {
      espera = 0;
      step = 0;
    }
  }

  int n = readkey();
  if (n == 2) xp = (xp <= 0 ? 0 : xp - 1);
  if (n == 4) xp = (xp >= 32 ? 32 : xp + 1);
  if (n == 1) {
    mood = (mood >= 7 ? 0 : mood + 1);
    do {} while (readkey() != 0);
  }
  if (n != 0) {
    espera = 0;
    step = 0;
  }
}

unsigned char readkey(void) {
  unsigned char ret = 0;
  if (digitalRead(8) == LOW) ret = 1;
  if (digitalRead(9) == LOW) ret = 2;
  if (digitalRead(10) == LOW) ret = 3;
  if (digitalRead(11) == LOW) ret = 4;
  return ret;
}

unsigned int readUltrasonicDistance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  unsigned int distance = duration / 29 / 2;
  return distance;
}

void moveForward() {
  digitalWrite(LEFT_MOTOR_FORWARD_PIN, HIGH);
  digitalWrite(LEFT_MOTOR_BACKWARD_PIN, LOW);
  digitalWrite(RIGHT_MOTOR_FORWARD_PIN, HIGH);
  digitalWrite(RIGHT_MOTOR_BACKWARD_PIN, LOW);
  Serial.println(F("Moving forward..."));
}

void moveBackward() {
  digitalWrite(LEFT_MOTOR_FORWARD_PIN, LOW);
  digitalWrite(LEFT_MOTOR_BACKWARD_PIN, HIGH);
  digitalWrite(RIGHT_MOTOR_FORWARD_PIN, LOW);
  digitalWrite(RIGHT_MOTOR_BACKWARD_PIN, HIGH);
  Serial.println(F("Moving backward..."));
}

void stopMovement() {
  digitalWrite(LEFT_MOTOR_FORWARD_PIN, LOW);
  digitalWrite(LEFT_MOTOR_BACKWARD_PIN, LOW);
  digitalWrite(RIGHT_MOTOR_FORWARD_PIN, LOW);
  digitalWrite(RIGHT_MOTOR_BACKWARD_PIN, LOW);
  Serial.println(F("Movement stopped..."));
}

void expressEmotion(Emotion emotion) {
  switch (emotion) {
    case HAPPY:
      displayEmotionBitmap(HAPPY);
      // Make a happy sound with the buzzer
      digitalWrite(BUZZER_PIN, HIGH);
      delay(100);
      digitalWrite(BUZZER_PIN, LOW);
      break;
    case SAD:
      displayEmotionBitmap(SAD);
      // Make a sad sound with the buzzer
      digitalWrite(BUZZER_PIN, HIGH);
      delay(300);
      digitalWrite(BUZZER_PIN, LOW);

      break;
    case ANGRY:
      displayEmotionBitmap(ANGRY);
      break;
    case NEUTRAL:
      displayEmotionBitmap(NEUTRAL);
      break;
  }
}

void displayEmotionBitmap(Emotion emotion) {
  display.clearDisplay();
  switch (emotion) {

    case HAPPY:
      display.drawBitmap(0, 0, happy_bitmap, eyeWidth, eyeHeight, WHITE);       // Eye 1
      //display.drawBitmap(32, 0, happy_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 2
      //display.drawBitmap(64, 0, happy_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 3
      //display.drawBitmap(96, 0, happy_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 4
      //display.drawBitmap(0, 32, happy_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 5
      //display.drawBitmap(32, 32, happy_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 6
      //display.drawBitmap(64, 32, happy_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 7
      //display.drawBitmap(96, 32, happy_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 8
      break;
/*
    case SAD:
      display.drawBitmap(0, 0, sad_bitmap, eyeWidth, eyeHeight, WHITE);         // Eye 1
      display.drawBitmap(32, 0, sad_bitmap, eyeWidth, eyeHeight, WHITE);        // Eye 2
      display.drawBitmap(64, 0, sad_bitmap, eyeWidth, eyeHeight, WHITE);        // Eye 3
      display.drawBitmap(96, 0, sad_bitmap, eyeWidth, eyeHeight, WHITE);        // Eye 4
      display.drawBitmap(0, 32, sad_bitmap, eyeWidth, eyeHeight, WHITE);        // Eye 5
      display.drawBitmap(32, 32, sad_bitmap, eyeWidth, eyeHeight, WHITE);       // Eye 6
      display.drawBitmap(64, 32, sad_bitmap, eyeWidth, eyeHeight, WHITE);       // Eye 7
      display.drawBitmap(96, 32, sad_bitmap, eyeWidth, eyeHeight, WHITE);       // Eye 8
      break;
    case ANGRY:
      display.drawBitmap(0, 0, angry_bitmap, eyeWidth, eyeHeight, WHITE);       // Eye 1
      display.drawBitmap(32, 0, angry_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 2
      display.drawBitmap(64, 0, angry_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 3
      display.drawBitmap(96, 0, angry_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 4
      display.drawBitmap(0, 32, angry_bitmap, eyeWidth, eyeHeight, WHITE);      // Eye 5
      display.drawBitmap(32, 32, angry_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 6
      display.drawBitmap(64, 32, angry_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 7
      display.drawBitmap(96, 32, angry_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 8
      break;
    case NEUTRAL:
      display.drawBitmap(0, 0, neutral_bitmap, eyeWidth, eyeHeight, WHITE);     // Eye 1
      display.drawBitmap(32, 0, neutral_bitmap, eyeWidth, eyeHeight, WHITE);    // Eye 2
      display.drawBitmap(64, 0, neutral_bitmap, eyeWidth, eyeHeight, WHITE);    // Eye 3
      display.drawBitmap(96, 0, neutral_bitmap, eyeWidth, eyeHeight, WHITE);    // Eye 4
      display.drawBitmap(0, 32, neutral_bitmap, eyeWidth, eyeHeight, WHITE);    // Eye 5
      display.drawBitmap(32, 32, neutral_bitmap, eyeWidth, eyeHeight, WHITE);   // Eye 6
      display.drawBitmap(64, 32, neutral_bitmap, eyeWidth, eyeHeight, WHITE);   // Eye 7
      display.drawBitmap(96, 32, neutral_bitmap, eyeWidth, eyeHeight, WHITE);   // Eye 8
      break;
      */
  }
  display.display();
}

void reactToCommand(String command) {
  if (command.equalsIgnoreCase("forward")) {
    moveForward();
  } else if (command.equalsIgnoreCase("backward")) {
    moveBackward();
  } else if (command.equalsIgnoreCase("stop")) {
    stopMovement();
  } else {
    morseCodeInterpreter(command[0]); // Assuming single character commands
  }
}

void sayYes() {
  for (int i = 0; i < 1; i++) {
    digitalWrite(BUZZER_PIN, HIGH);
    delay(100);
    digitalWrite(BUZZER_PIN, LOW);
    delay(100);
  }
}

void sayNo() {
  for (int i = 0; i < 1; i++) {
    digitalWrite(BUZZER_PIN, HIGH);
    delay(300);
    digitalWrite(BUZZER_PIN, LOW);
    delay(100);
  }
}

String readSerialInput() {
  static String commandBuffer = "";
  while (Serial.available() > 0) {
    char c = Serial.read();
    if (c == '\n') {
      String command = commandBuffer;
      commandBuffer = "";
      return command;
    } else {
      commandBuffer += c;
    }
  }
  return "";
}

void avoidObstacle() {
  unsigned int distance = readUltrasonicDistance();
  if (distance < 10) {
    stopMovement();
    moveBackward();
    delay(500);
    stopMovement();
    delay(500);
    moveForward();
  } else {
    moveForward();
  }
}

void moveArms(Servo& leftServo, Servo& rightServo, int angle) {
  leftServo.write(angle);
  rightServo.write(angle);
  delay(500);
}

String readMorseInput() {
  static String morseBuffer = ""; // Buffer to store Morse code input
  static unsigned long lastInputTime = 0; // Timestamp of last input

  // Simulated input detection (replace with actual sensor or button input)
  if (digitalRead(MORSE_INPUT_PIN) == LOW) {
    if (millis() - lastInputTime > 200) { // Time threshold between inputs
      morseBuffer += "."; // Simulated dot (.)
      lastInputTime = millis();
    }
  } else {
    if (morseBuffer.length() > 0) {
      return morseBuffer;
    }
  }

  return ""; // Return empty string if no complete Morse code input
}

void morseCodeInterpreter(char character) {
  switch (character) {
    case '.':
      expressEmotion(HAPPY);
      Serial.println(F("Morse code interpreted: . (Happy)"));
      break;
    case '-':
      expressEmotion(SAD);
      Serial.println(F("Morse code interpreted: - (Sad)"));
      break;
    // Add more cases as needed for other Morse code inputs
    default:
      Serial.println(F("Unknown Morse code"));
      break;
  }
}