#include <Servo.h> // Include the Servo library for controlling the ASMC-LQB

// --- Pin Definitions ---
// Input from ASMC-LQB Servo Position Feedback
const int SERVO_FEEDBACK_PIN = A0; // Connect the ASMC-LQB's analog feedback signal here

// Output to ECU for Position Confirmation (S1, S2, S3, S4)
const int S1_PIN = 2; // Digital pin for S1 output
const int S2_PIN = 3; // Digital pin for S2 output
const int S3_PIN = 4; // Digital pin for S3 output
const int S4_PIN = 5; // Digital pin for S4 output

// Input from Transfer Case ECU for Servo Movement Commands
// Connect these pins to the OUTPUT of your 12V-to-5V voltage divider circuits
const int ECU_FWD_COMMAND_PIN = 6; // Digital input for "Move Forward" command from ECU
const int ECU_REV_COMMAND_PIN = 7; // Digital input for "Move Reverse" command from ECU

// Output to ASMC-LQB Servo Control
// ASMC-LQB typically uses a standard servo PWM signal
const int SERVO_CONTROL_PIN = 9; // PWM-capable pin for ASMC-LQB control signal

// --- Servo Object ---
Servo asmcServo; // Create a servo object to control the ASMC-LQB

// --- Servo Control Parameters ---
// These define the MIN and MAX angles the ASMC-LQB should physically move
// You MUST calibrate these based on your mechanical setup and the ASMC-LQB's capabilities.
// The ASMC-LQB is 0-360 degrees, but your mechanical linkage might use a smaller range.
const int MIN_SERVO_ANGLE = 0;   // Angle for "Left Stop" position
const int MAX_SERVO_ANGLE = 360; // Angle for "Right Stop" position
const int SERVO_STEP_SIZE = 1;   // How many degrees to move per update cycle (adjust for speed)

// Variable to store the desired target angle for the ASMC-LQB
int targetServoAngle;

// --- Position Thresholds for ECU Feedback (CALIBRATE THESE VALUES!) ---
// These are placeholder values. You MUST calibrate these based on the actual
// analog readings you get from your servo at each defined position.
// AnalogRead returns values from 0 to 1023 for a 0-5V input.

// Example: If Left Stop typically reads from 0 to 100, set LEFT_STOP_MAX_READING to 100.
// Then the next mode starts from 101, and so on.
// Note: These mappings need to correspond to the physical positions your ASMC-LQB reaches.
const int LEFT_STOP_MAX_READING        = 100;  // Max reading for "Left Stop"
const int LEFT_OF_HI_MODE_MAX_READING  = 200;  // Max reading for "Left of Hi Mode"
const int HI_MODE_MAX_READING          = 300;  // Max reading for "Hi Mode"
const int RIGHT_OF_HI_MODE_MAX_READING = 400;  // Max reading for "Right of Hi Mode"
const int ZONE1_MAX_READING            = 500;  // Max reading for "Zone 1"
const int NEUTRAL_MODE_MAX_READING     = 600;  // Max reading for "Neutral Mode"
const int ZONE2_MAX_READING            = 700;  // Max reading for "Zone 2"
const int LO_MODE_MAX_READING          = 800;  // Max reading for "Lo Mode"
const int RIGHT_STOP_MAX_READING       = 1023; // Max reading for "Right Stop" (covers remaining range)

// --- Setup Function ---
void setup() {
  // Initialize Serial communication for debugging and calibration
  Serial.begin(9600);
  Serial.println("Arduino Servo Position and Control Mapper Started!");
  Serial.println("------------------------------------");
  Serial.println("CALIBRATION IS CRITICAL for both Servo Control Angles and ECU Feedback Thresholds.");
  Serial.println("1. For Servo Control: Manually determine MIN_SERVO_ANGLE and MAX_SERVO_ANGLE based on your linkage.");
  Serial.println("2. For ECU Feedback: Observe 'Current Reading' in Serial Monitor while manually moving the servo.");
  Serial.println("   Adjust the LEFT_STOP_MAX_READING, etc., thresholds accordingly.");
  Serial.println("------------------------------------");

  // Attach the ASMC-LQB servo to its control pin
  asmcServo.attach(SERVO_CONTROL_PIN);

  // Set initial target angle (e.g., to neutral or minimum)
  targetServoAngle = (MIN_SERVO_ANGLE + MAX_SERVO_ANGLE) / 2; // Start in the middle
  // Or, if you want it to start at a known "neutral" position, map that position's analog
  // reading to an angle and set targetServoAngle to that.
  asmcServo.write(targetServoAngle); // Command the servo to its initial position

  // Set the output pins for ECU signals as OUTPUTs
  pinMode(S1_PIN, OUTPUT);
  pinMode(S2_PIN, OUTPUT);
  pinMode(S3_PIN, OUTPUT);
  pinMode(S4_PIN, OUTPUT);

  // Set the input pins for ECU commands as INPUTs
  pinMode(ECU_FWD_COMMAND_PIN, INPUT);
  pinMode(ECU_REV_COMMAND_PIN, INPUT);

  // Ensure initial state of S1-S4 is defined (e.g., all switches open/HIGH)
  digitalWrite(S1_PIN, HIGH);
  digitalWrite(S2_PIN, HIGH);
  digitalWrite(S3_PIN, HIGH);
  digitalWrite(S4_PIN, HIGH);
}

// --- Loop Function ---
void loop() {
  // --- Part 1: Control ASMC-LQB based on ECU Commands ---
  bool ecuFwdActive = digitalRead(ECU_FWD_COMMAND_PIN) == HIGH;
  bool ecuRevActive = digitalRead(ECU_REV_COMMAND_PIN) == HIGH;

  if (ecuFwdActive && !ecuRevActive) {
    // ECU wants to move "forward" (e.g., towards Right Stop)
    targetServoAngle += SERVO_STEP_SIZE;
    if (targetServoAngle > MAX_SERVO_ANGLE) {
      targetServoAngle = MAX_SERVO_ANGLE; // Cap at max angle
    }
  } else if (ecuRevActive && !ecuFwdActive) {
    // ECU wants to move "reverse" (e.g., towards Left Stop)
    targetServoAngle -= SERVO_STEP_SIZE;
    if (targetServoAngle < MIN_SERVO_ANGLE) {
      targetServoAngle = MIN_SERVO_ANGLE; // Cap at min angle
    }
  }
  // If neither or both are active, targetServoAngle holds its current value (servo stops moving)

  // Command the ASMC-LQB servo to the calculated target angle
  asmcServo.write(targetServoAngle);

  // --- Part 2: Provide ASMC-LQB Position Feedback to ECU ---
  // Read the analog value from the servo feedback pin
  int currentPositionReading = analogRead(SERVO_FEEDBACK_PIN);

  // Variables to store the desired output states for S1, S2, S3, S4
  int s1_state, s2_state, s3_state, s4_state;

  // Determine the current mode based on analog reading and set output states
  // 1 = Switch Open (HIGH), 0 = Switch Closed (LOW)
  if (currentPositionReading <= LEFT_STOP_MAX_READING) {
    // Left Stop: S1=1, S2=1, S3=1, S4=0
    s1_state = HIGH;
    s2_state = HIGH;
    s3_state = HIGH;
    s4_state = LOW;
    Serial.print("Position: Left Stop (Reading: ");
  } else if (currentPositionReading <= LEFT_OF_HI_MODE_MAX_READING) {
    // Left of Hi Mode: S1=1, S2=0, S3=1, S4=0
    s1_state = HIGH;
    s2_state = LOW;
    s3_state = HIGH;
    s4_state = LOW;
    Serial.print("Position: Left of Hi Mode (Reading: ");
  } else if (currentPositionReading <= HI_MODE_MAX_READING) {
    // Hi Mode: S1=0, S2=0, S3=1, S4=0
    s1_state = LOW;
    s2_state = LOW;
    s3_state = HIGH;
    s4_state = LOW;
    Serial.print("Position: Hi Mode (Reading: ");
  } else if (currentPositionReading <= RIGHT_OF_HI_MODE_MAX_READING) {
    // Right of Hi Mode: S1=0, S2=0, S3=0, S4=0
    s1_state = LOW;
    s2_state = LOW;
    s3_state = LOW;
    s4_state = LOW;
    Serial.print("Position: Right of Hi Mode (Reading: ");
  } else if (currentPositionReading <= ZONE1_MAX_READING) {
    // Zone 1: S1=1, S2=0, S3=0, S4=0
    s1_state = HIGH;
    s2_state = LOW;
    s3_state = LOW;
    s4_state = LOW;
    Serial.print("Position: Zone 1 (Reading: ");
  } else if (currentPositionReading <= NEUTRAL_MODE_MAX_READING) {
    // Neutral Mode: S1=1, S2=0, S3=0, S4=1
    s1_state = HIGH;
    s2_state = LOW;
    s3_state = LOW;
    s4_state = HIGH;
    Serial.print("Position: Neutral Mode (Reading: ");
  } else if (currentPositionReading <= ZONE2_MAX_READING) {
    // Zone 2: S1=0, S2=0, S3=0, S4=1
    s1_state = LOW;
    s2_state = LOW;
    s3_state = LOW;
    s4_state = HIGH;
    Serial.print("Position: Zone 2 (Reading: ");
  } else if (currentPositionReading <= LO_MODE_MAX_READING) {
    // Lo Mode: S1=0, S2=1, S3=0, S4=1
    s1_state = LOW;
    s2_state = HIGH;
    s3_state = LOW;
    s4_state = HIGH;
    Serial.print("Position: Lo Mode (Reading: ");
  } else { // currentPositionReading > LO_MODE_MAX_READING, up to 1023
    // Right Stop: S1=0, S2=1, S3=0, S4=0
    s1_state = LOW;
    s2_state = HIGH;
    s3_state = LOW;
    s4_state = LOW;
    Serial.print("Position: Right Stop (Reading: ");
  }

  // Write the determined states to the output pins
  digitalWrite(S1_PIN, s1_state);
  digitalWrite(S2_PIN, s2_state);
  digitalWrite(S3_PIN, s3_state);
  digitalWrite(S4_PIN, s4_state);

  // Print current state to Serial Monitor for debugging and calibration
  Serial.print(currentPositionReading);
  Serial.print(") -> S1:"); Serial.print(s1_state);
  Serial.print(" S2:"); Serial.print(s2_state);
  Serial.print(" S3:"); Serial.print(s3_state);
  Serial.print(" S4:"); Serial.print(s4_state);
  Serial.print(" | Servo Angle: "); Serial.println(targetServoAngle);

  // Small delay to regulate motion and output updates
  delay(50);
}