#include <Servo.h>
#include <LiquidCrystal.h>

#define LATCH_PIN 9  // CI 12
#define CLOCK_PIN 10 // CI 11
#define DATA_PIN 8   // CI 14
#define numberOfControlPins 9

//Missile Flight Navigation data
static int numCheckPoints = 10;
static float Lats[12] = {33.448, 33.377, 33.217, 33.151, 32.962, 32.861, 32.804, 32.643, 32.544, 32.471, 32.364, 32.223};
static float Longs[12] = {-112.074, -111.932, -111.871, -111.79, -111.721, -111.526, -111.44, -111.354, -111.253, -111.167, -111.08, -110.975};
static float Dist[11] = {15.392, 18.592, 10.576, 22.008, 21.361, 10.262, 19.591, 14.476, 11.448, 14.431, 18.586};
static float Bear[11] = {121.2, 162.3, 134.4, 162.9, 121.5, 128.5, 155.7, 139.3, 135.4, 145.4, 147.8};
static float MCC[11] = {15, 18, 1057, 2200, 2136, 1026, 1959, 1447, 1144, 1443, 1858};
static float SAA[11] = {90, 131.1, 62.1, 118.5, 48.6, 97.0, 117.2, 73.6, 86.1, 100.0, 92.4};

// missile Command regulates what actions gets executed
static int missileCommand = 0;
static int missileNAVCycle = 0;
static int checkPointCycle = 0;

// Declare arrays to hold the echo pins and the corresponding distances
static int ultrasonicEchoPins[4] = {3, 4, 5, 6};
static float ultrasonicReadings[4] = {0, 0, 0, 0};
static int controlPins[10] = {37,35,33,31,29,27,25,23,22, 13};
// Main Power{1}, Stabilizers{2}, Tilt Switch{3}, GPS{4}, 
// Ultrasonic{5}, PIR{6}, Safety{7}, Arm{8}, CalibrateFins{9}, LAUNCH {10}

//Global Missile States
static bool Armed = false;
static bool Launched = false;
static bool Calibrated = false;


static int LCDPins[6] = {48,46,44,42,40,38};
LiquidCrystal lcd(LCDPins[0], LCDPins[1], LCDPins[2], LCDPins[3], LCDPins[4], LCDPins[5]);
static int globalFinState = 1;
static float angleInstructions[8] = {45, 23, 120, 83, 9, 79, 123, 76};
static Servo servos[4];
static int servoPins[4] = {39,41,43,45};
// +Z+X{39}, +Z-X{41}, -Z+X{43}, -Z-X{45}

void resetNAVCycle() {
  missileNAVCycle = 0;
}

void nextNAVCycle() {
  missileNAVCycle += 1;
}

// Calibrates the fin
void calibrateFin() {
  // Allow time for button to unpress
  delay(100);
  for (int i = 0; i < 4; i++) {
    servos[i].attach(servoPins[i]);
  }
  for (int i = 0; i < 4; i++) {
    rotateServo(servos[i], 180);
    delay(100);
    rotateServo(servos[i], 90);
  }
}

// Function to rotate the fins
void rotateServo(Servo &servo, float angle) {
  servo.write(angle);
}

// Adjust the fins based on the the angle instructions
void adjustFins() {
  int fin = 0;
  for (int i = (globalFinState-1)*4; i <= ((globalFinState-1)*4)+3; i++) {
    Serial.println(angleInstructions[i]);
    rotateServo(servos[fin],angleInstructions[i]);
    delay(100);
    fin += 1;
  }

  globalFinState += 1;
}

// Function to send data to Shift Register
void dataToShiftRegister(int dataToSend) {
  digitalWrite(LATCH_PIN, LOW); // Prepare to send data
  shiftOut(DATA_PIN, CLOCK_PIN, LSBFIRST, dataToSend); // Send data
  digitalWrite(LATCH_PIN, HIGH); // Latch the data
  delay(100);
}

// Function to read distance from a single ultrasonic sensor
float readUltrasonicDistance(int trigPin, int echoPin) {
    long duration;
    float distance;

    // Send a 10-microsecond pulse to the Trigger pin
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // Measure the duration of the Echo pin HIGH signal
    duration = pulseIn(echoPin, HIGH);

    // Calculate distance in cm
    distance = (duration / 2.0) * 0.0343;
    return distance;
}


// Output the values for debugging
void readPIRData(int inputPin) {
  int data = digitalRead(inputPin);
  if (data == HIGH) {
    Serial.println("Motion Detected");
  }
  else {
    Serial.println("No Motion Detect");
  }
  data = LOW;
  
}
// Function to collect distances from all ultrasonic sensors
void collectUltrasonicReadings(int trigPin) {
    for (int sensor = 0; sensor < 4; sensor++) {
        // Collect distance and store it in the array
        ultrasonicReadings[sensor] = readUltrasonicDistance(trigPin, ultrasonicEchoPins[sensor]);
    }
}
// Display the NAV Data
void displayNAVData(int indexVal) {
    lcd.clear();
    lcd.print("Target GPS: ");
    lcd.setCursor(0, 1);
    lcd.print(Lats[indexVal]);
    lcd.print(" ");
    lcd.print(Longs[indexVal]);

}

void navControlFins() {
  for (int i = 0; i < 4; i++) {
    rotateServo(servos[i], SAA[checkPointCycle]);
    delay(100);
  }
  
}
// Function to print the array of distances for debugging
void printUltrasonicReadings() {
    Serial.print("Ultrasonic Readings: ");
    for (int i = 0; i < 4; i++) {
        Serial.print(ultrasonicReadings[i]);
        Serial.print(" cm ");
    }
    Serial.println(); // Move to a new line
}

void checkIfTurnedOff() {
if (digitalRead(controlPins[0]) == LOW) {
    Serial.println("Switch turned off. Stopping sequence.");
    missileCommand = 0;
  }
}
void setup() {
    Serial.begin(9600);
    lcd.begin(16, 2);
    calibrateFin();
    // Set pin modes for shift register and ultrasonic sensors
    pinMode(LATCH_PIN, OUTPUT);
    pinMode(CLOCK_PIN, OUTPUT);
    pinMode(DATA_PIN, OUTPUT);
    missileCommand = 1;
    pinMode(2, OUTPUT); // Trigger pin for ultrasonic sensors
    digitalWrite(2, LOW); // Ensure Trigger pin starts LOW

    for (int i = 0; i < 4; i++) {
        pinMode(ultrasonicEchoPins[i], INPUT); // Echo pins
    }
    // Activate the power pins
    for (int i = 0; i <= numberOfControlPins; i++) {
      pinMode(controlPins[i], INPUT);
    }

}

void loop() {
// Inputs: Idle{0}, Scan Ultrasonic {1}, Get GPS {2}, Get Tilt {3}, Get PIR {4}
// States: Calibrate {4}, Reset{5}, Arm{6}, Disarm{7}
// Ouputs: Shift Register {8}, Calibrate Fins {9}, adjustFins {10}
// NAVIGATION: Ouput TargetGPS {11}, OutPut Bearings {12}, Update MNC {11}
// Nothing runs unless the Main Power pin is set to on
if (digitalRead(controlPins[0]) == HIGH) {
  //Start up sequence:
  //Power -> Fins -> Calibrate -> Ultrasonic -> PIR -> Update Servos if launched
    // If Ultrasonics are turned on and recieve command
    if (digitalRead(controlPins[4]) == HIGH && missileCommand == 1) {
        checkIfTurnedOff();
        collectUltrasonicReadings(2);
        printUltrasonicReadings();

      for (int count = 0; count < 4; count++) {
            // Convert the ultrasonic reading to a value that can be sent to the shift register
            int dataToSend = map((int)ultrasonicReadings[count], 0, 400, 0, 255); // Map to 8-bit range (0–255)
            dataToShiftRegister(dataToSend);
      }
      missileCommand = 4;

    }

    // If calibarate and fins turned on, Calibrate fins
    if (digitalRead(controlPins[8]) == HIGH && digitalRead(controlPins[1])) {
        checkIfTurnedOff();
        calibrateFin();
        Calibrated = true;
        // Send the command to the ultrasonic sensors
        //missileCommand = 1;

    }

    // If PIR Sensor is turned on and received command 4
    if (digitalRead(controlPins[5]) == HIGH && missileCommand == 4) {
      checkIfTurnedOff();
      readPIRData(11);
      missileCommand = 11;
    }

    // If 3 is turned on and recieves command 10, go to next instruction
    if (digitalRead(controlPins[2]) == HIGH && missileCommand == 10) {
      checkIfTurnedOff();
      adjustFins();

    }

    // If the NAV Cyle is equal to the Missile Check Point Cylce
    if (digitalRead(controlPins[3]) == HIGH && missileCommand == 11) {
      checkIfTurnedOff();
      int arraySize = sizeof(MCC) / sizeof(MCC[0]);
      if (missileNAVCycle >= MCC[checkPointCycle] && checkPointCycle < arraySize) {
        Serial.println(MCC[checkPointCycle]);
        displayNAVData(checkPointCycle);
        navControlFins();
        resetNAVCycle();
        checkPointCycle += 1;
      }
      missileCommand = 1;
    }

    if (digitalRead(controlPins[7]) == HIGH && digitalRead(controlPins[6]) == HIGH) {
      Armed = true;
    }
    else {
      Armed = false;
    }

    if (digitalRead(controlPins[9]) == HIGH) {
      Serial.println("LAUNCHED!");
      Launched = true;
  
  }
  //missileCommand = 11;
  // Delay only for simulation stability. 
  if (Launched) {
    nextNAVCycle();
  }
  delay(100);
}

}