//////////////////////////////////////////////////////////
//                         C|A|T                        //
//              COSMIC AEROSPACE TECHNOLOGY             //
//                FC-Udev Flight Software               //
//                    June  26th 2023                   //
//                        CANADA                        //
//                  BY SHAYAN HOGHOOGHI                 //
//////////////////////////////////////////////////////////

//Including Libraries
#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <math.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_BMP280.h>

//Values for Inertial Measurment Unit, it is recommended not to touch these or to create duplicates
unsigned long timer = 0;
const int MPU_ADDR = 0x68;
float roll, pitch, yaw, Ax, Ay, Az, Gx, Gy;
float elapsedTime = 0, time, previousTime = 0, currentTime;
int16_t alpha1 = 0.85;
int16_t alpha2 = 0.5;
int16_t AcX, AcY, AcZ;
const int MPU_addr = 0x68;

int minVal = 265;
int maxVal = 402;

//Potentiometer Variable
float potVal;

//Continuity Data/LEDs
int cnt1 = 9;
int cnt2 = 10;
int p1 = 11;
int p2 = 12;
int read1, read2;

//Pyro-Channel Terminal Pins
int pyro1 = 7;
int pyro2 = 8;

//State LEDs Pins
const int grn = 4;
const int red = 5;
const int blu = 6;

//initial LED Values (off)
int r = 0;
int g = 0;
int b = 0;
int buz = 0;

int buzzer = 99; //Change to your desired buzzer pin{Off by Default}
//int BuiltinLED = 13; //Builtin LED
int state;

//Tuning and Filtering Barometer
float initialAltitude;
float PressureAtSea = 1003.80; //CHANGE VALUE DEPENDING ON YOUR LOCATION
float maxAltitude = 0;

//Defining Object Sensors
Adafruit_MPU6050 mpu;
Adafruit_BMP280 bmp;

void setup()
{
    // put your setup code here, to run once:

    Wire.begin(); //begin the wire comunication
    Wire.beginTransmission(0x68); //Calling Registers
    Wire.write(0x6B);
    Wire.write(0);
    Wire.endTransmission(true);

    //Waking up I2C registers for MPU and BMP
    !mpu.begin();
    !bmp.begin(0x76);
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

    //Calibrating Settings for BMP280 from the Adafruit Library
    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, /*Operating Mode. */
        Adafruit_BMP280::SAMPLING_X1,             /*Temp. oversampling */
        Adafruit_BMP280::SAMPLING_X1,             /*Pressure oversampling */
        Adafruit_BMP280::FILTER_X16,              /*Filtering. */
        Adafruit_BMP280::STANDBY_MS_1);           /*Standby time. */

    Serial.begin(115200);

    pinMode(red, OUTPUT);
    pinMode(blu, OUTPUT);
    pinMode(grn, OUTPUT);
    pinMode(buzzer, OUTPUT);
    pinMode(pyro1, OUTPUT);
    pinMode(pyro2, OUTPUT);
    pinMode(cnt1, OUTPUT);
    pinMode(cnt2, OUTPUT);
    pinMode(p1, OUTPUT);
    pinMode(p2, OUTPUT);

    startup();
    protocol();
}

void loop()
{
    previousTime = currentTime;
    currentTime = millis(); // Current time actual time read
    elapsedTime = (currentTime - previousTime) / 1000;
    previousTime = currentTime;

    // Calculate time elapsed since the last loop iteration
    unsigned long currentTime = millis();
    elapsedTime = (currentTime - previousTime) / 1000.0;
    previousTime = currentTime;

    // Read accelerometer values
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    launchdetect();
    int read1 = analogRead(A1);
    int read2 = analogRead(A2);
    if (read1 >= 200) {
        digitalWrite(cnt1, HIGH);
    }
    else {
        digitalWrite(cnt1, LOW);
    }

    if (read2 >= 200) {
        digitalWrite(cnt2, HIGH);
    }
    else {
        digitalWrite(cnt2, LOW);
    }

    Serial.flush();
}

void flight()
{
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    digitalWrite(red, HIGH);
    digitalWrite(blu, LOW);
    digitalWrite(grn, LOW);
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true);
    AcX = Wire.read() << 8 | Wire.read();
    AcY = Wire.read() << 8 | Wire.read();
    AcZ = Wire.read() << 8 | Wire.read();
    int xAng = map(AcX, minVal, maxVal, -90, 90);
    int yAng = map(AcY, minVal, maxVal, -90, 90);
    int zAng = map(AcZ, minVal, maxVal, -90, 90);

    //Get accelerometer data
    float accelX = a.acceleration.x;
    float accelY = a.acceleration.y;
    float accelZ = a.acceleration.z;
    float FilteredAccelX = (( alpha2 * FilteredAccelX ) + accelX ) / ( alpha2 + 1 );
    float FilteredAccelY = (( alpha2 * FilteredAccelY ) + accelY ) / ( alpha2 + 1 );
    float FilteredAccelZ = (( alpha2 * FilteredAccelZ ) + accelZ ) / ( alpha2 + 1 );


    //Complementary Filter to smooth Gyroscopic data
    roll = alpha1 * (roll + (g.gyro.x * elapsedTime)) + (1 - alpha1) * (atan2(-FilteredAccelY, -FilteredAccelZ) * RAD_TO_DEG);
    pitch = alpha1 * (pitch + (g.gyro.y * elapsedTime)) + (1 - alpha1) * (atan2(FilteredAccelX, sqrt(FilteredAccelY * FilteredAccelY + FilteredAccelZ * FilteredAccelZ)) * RAD_TO_DEG);
    yaw = alpha1 * (yaw + (g.gyro.z * elapsedTime)) + (1 - alpha1) * (atan2(-FilteredAccelY, -FilteredAccelZ) * RAD_TO_DEG);

    float x = roll + 180.0; // Convert roll to range 0-360
    float y = pitch + 180.0; // Convert pitch to range 0-360
    float z = yaw + 180.0; // Convert yaw to range 0-360
    float t = bmp.readTemperature();

    bmp.takeForcedMeasurement();
    int potVal = analogRead(A0);
    int TimeDelay = ((-(potVal) + 1023) / 102.3) * 1000;
    int read1 = analogRead(A1);
    int read2 = analogRead(A2);
    float CurrentAltitude = bmp.readAltitude(PressureAtSea) - initialAltitude;

    //============ - UNCOMMENT TO USE WITH CAtower -  ===============
    //Datapackets will grab the values of the corresponding variables

    Serial.print("@"); //New Data pack identifier
    Serial.print(x);
    Serial.print("x"); //gyro X
    Serial.print(y);
    Serial.print("y"); //gyro Y
    Serial.print(z);
    Serial.print("z"); //gyro Z
    Serial.print(FilteredAccelX);
    Serial.print("X"); //acclerometer X
    Serial.print(FilteredAccelY);
    Serial.print("Y"); //acclerometer Y
    Serial.print(FilteredAccelZ);
    Serial.print("Z"); //acclerometer Z
    Serial.print(t - 3.80);
    Serial.print("t"); //Temperature
    Serial.print(CurrentAltitude);
    Serial.print("A"); //Altitude (Auto sets to 0m during launch detect + or - 30cm)
    Serial.print(bmp.readPressure() / 100);
    Serial.print("P"); //Pressure
    Serial.print(read1);
    Serial.print("c"); //continuity 1
    Serial.print(read2);
    Serial.print("C"); //continuity 2
    Serial.print(state);
    Serial.print("S"); //State
    Serial.print(TimeDelay / 1000);
    Serial.print("T"); //Delay Time
    Serial.print("\n");

    //================ - Apogee detection by Altitude - ==================

    if (CurrentAltitude > maxAltitude) {
        maxAltitude = CurrentAltitude;
    }

    if (maxAltitude > CurrentAltitude + 0.75) {
        deploy();
    }
}

void startup()
{
    //LED gradient effect
    setColor(255, 0, 0, 0); //red
    setColor(0, 0, 255, 450); //blue
    setColor(255, 0, 255, 1800);
}

void setColor(int red, int green, int blue, int buzz)
{
    //Bootup-Light Effect
    while (r != red || g != green || b != blue || buz != buzz) {
        if (r < red)
            r += 1;
        if (r > red)
            r -= 1;

        if (g < green)
            g += 1;
        if (g > green)
            g -= 1;

        if (b < blue)
            b += 1;
        if (b > blue)
            b -= 1;

        if (buz < buzz)
            buz += 10;
        if (buz > buzz)
            buz -= 10;

        _setColor();
        delay(5);
    }

    if (buz == 1800) {
        //buzzer tone
        noTone(buzzer);
        delay(500);
        tone(buzzer, 1800, 100);
        delay(100);
        noTone(buzzer);
        tone(buzzer, 1800, 100);
        delay(100);
        noTone(buzzer);
    }
}

void _setColor()
{
    analogWrite(red, r);
    analogWrite(grn, g);
    analogWrite(blu, b);
    tone(buzzer, buz);

    //Calibrates Baro-Sensor to set initial altitude to 0m
    initialAltitude = bmp.readAltitude(PressureAtSea);
}

//================ - LaunchDetection - ===============
//
void launchdetect()
{
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    Ax = a.acceleration.x;
    Ay = a.acceleration.y;

    if (state == 0 && Ay > 60) {
        //Default set to 60m/s^2
        state++;
    }

    if (state == 1) {
        flight();
    }
}

void protocol()
{
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    Ax = a.acceleration.x;
    Ay = a.acceleration.y;
    Gx = g.gyro.x;
    Gy = g.gyro.y;

    state == 0;
    delay(750);
    Serial.println("Polaris FC-102 is Ready for Flight");
    int statusA;
    statusA = (a.acceleration.x + a.acceleration.y) / 2;
    if (statusA = 0) {
        Serial.println("Accel Initialization Error");
        while (1) {
        }
    }

    int statusG;
    statusG = (g.gyro.x + g.gyro.y) / 2;
    if (statusG = 0) {
        Serial.println("Gyro Initialization Error");
        while (1) {
        }
    }

    delay(500);
    digitalWrite(grn, HIGH);
    digitalWrite(red, LOW);
    digitalWrite(blu, HIGH);
}

void deploy()
{
    int potVal = analogRead(A0);
    int TimeDelay = ((-(potVal) + 1023) / 102.3) * 1000;
    digitalWrite(red, HIGH);
    digitalWrite(grn, HIGH);
    Serial.print("\n");
    delay(TimeDelay);
    state++;
    digitalWrite(pyro1, HIGH);
    digitalWrite(pyro2, HIGH);
    digitalWrite(p1, HIGH);
    digitalWrite(p2, HIGH);
    tone(buzzer, 1800);
    delay(5000);
    state++;
    while (state == 3) {
        digitalWrite(pyro1, LOW);
        digitalWrite(pyro2, LOW);
        digitalWrite(red, HIGH);
        digitalWrite(blu, HIGH);
        digitalWrite(grn, HIGH);
        tone(buzzer, 2000, 500);
        delay(500);
        digitalWrite(red, HIGH);
        digitalWrite(blu, LOW);
        digitalWrite(grn, LOW);
        delay(100);
    }
}