/*
 * Arduino Mini Drawing Robot Arm
 * Components: Stepper Motor (A4988), 2x Servos, LCD I2C, 2x Potentiometers, RGB LED, LED
 * Difficulty: Beginner-Intermediate
 */

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Pin definitions
const int STEP_PIN = 3;
const int DIR_PIN = 4;
const int RGB_RED = 5;
const int RGB_GREEN = 6;
const int RGB_BLUE = 7;
const int STATUS_LED = 8;
const int SERVO1_PIN = 9;
const int SERVO2_PIN = 10;
const int POT1_PIN = A0;
const int POT2_PIN = A1;

// Create objects
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Address 0x27, 16 chars, 2 lines
Servo servo1;  // Arm servo
Servo servo2;  // Gripper servo

// Variables
int stepperPosition = 0;
int servo1Angle = 90;
int servo2Angle = 90;
unsigned long lastUpdate = 0;
bool ledState = false;

void setup() {
  // Initialize pins
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(RGB_RED, OUTPUT);
  pinMode(RGB_GREEN, OUTPUT);
  pinMode(RGB_BLUE, OUTPUT);
  pinMode(STATUS_LED, OUTPUT);
  
  // Initialize servos
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo1.write(90);  // Center position
  servo2.write(90);
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Robot Arm v1.0");
  lcd.setCursor(0, 1);
  lcd.print("Initializing...");
  
  // Welcome RGB sequence
  setRGBColor(255, 0, 0);    // Red
  delay(300);
  setRGBColor(0, 255, 0);    // Green
  delay(300);
  setRGBColor(0, 0, 255);    // Blue
  delay(300);
  setRGBColor(0, 255, 255);  // Cyan (Ready)
  
  delay(1000);
  lcd.clear();
}

void loop() {
  // Read potentiometer values
  int pot1Value = analogRead(POT1_PIN);
  int pot2Value = analogRead(POT2_PIN);
  
  // Map potentiometer values to angles/positions
  int targetStepperPos = map(pot1Value, 0, 1023, -200, 200);
  int targetServo1Angle = map(pot2Value, 0, 1023, 0, 180);
  
  // Automatic gripper movement (oscillates between open and closed)
  unsigned long currentTime = millis();
  if (currentTime - lastUpdate > 2000) {
    servo2Angle = (servo2Angle == 60) ? 120 : 60;  // Toggle gripper
    servo2.write(servo2Angle);
    lastUpdate = currentTime;
    
    // Blink status LED
    ledState = !ledState;
    digitalWrite(STATUS_LED, ledState);
  }
  
  // Control stepper motor smoothly
  if (stepperPosition < targetStepperPos) {
    digitalWrite(DIR_PIN, HIGH);
    stepMotor(1);
    stepperPosition++;
  } else if (stepperPosition > targetStepperPos) {
    digitalWrite(DIR_PIN, LOW);
    stepMotor(1);
    stepperPosition--;
  }
  
  // Control servo 1 smoothly
  if (servo1Angle < targetServo1Angle) {
    servo1Angle++;
    servo1.write(servo1Angle);
    delay(15);
  } else if (servo1Angle > targetServo1Angle) {
    servo1Angle--;
    servo1.write(servo1Angle);
    delay(15);
  }
  
  // Update RGB LED based on position
  updateRGBBasedOnPosition(servo1Angle);
  
  // Update LCD display
  updateDisplay(stepperPosition, servo1Angle, servo2Angle);
  
  delay(10);  // Small delay for stability
}

// Function to step the motor
void stepMotor(int steps) {
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(1000);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(1000);
  }
}

// Function to set RGB LED color
void setRGBColor(int red, int green, int blue) {
  analogWrite(RGB_RED, red);
  analogWrite(RGB_GREEN, green);
  analogWrite(RGB_BLUE, blue);
}

// Update RGB color based on servo position
void updateRGBBasedOnPosition(int angle) {
  if (angle < 60) {
    setRGBColor(255, 0, 0);      // Red - Low position
  } else if (angle < 120) {
    setRGBColor(255, 255, 0);    // Yellow - Mid position
  } else {
    setRGBColor(0, 255, 0);      // Green - High position
  }
}

// Update LCD display with current information
void updateDisplay(int base, int arm, int grip) {
  static unsigned long lastLCDUpdate = 0;
  
  // Update LCD every 200ms to reduce flicker
  if (millis() - lastLCDUpdate > 200) {
    lcd.setCursor(0, 0);
    lcd.print("Base:");
    lcd.print(base);
    lcd.print("   ");  // Clear extra characters
    
    lcd.setCursor(10, 0);
    lcd.print("A:");
    lcd.print(arm);
    lcd.print("  ");
    
    lcd.setCursor(0, 1);
    lcd.print("Grip:");
    lcd.print(grip == 60 ? "OPEN  " : "CLOSED");
    
    lastLCDUpdate = millis();
  }
}