#include "Standard_Includes.h"

//#define DEBUG_OUTPUT

#if defined(DEBUG_OUTPUT)
#define DEBUG_PRINT(x)   Serial1.print(x)
#define DEBUG_PRINTLN(x) Serial1.println(x)
#define DEBUG_SERIAL_BEGIN(x) Serial1.begin(x)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTLN(x)
#define DEBUG_SERIAL_BEGIN(x)
#endif

// Constants
#define ENCODER_SAMPLESIZE 3
#define MOTOR_MAXSPEED 255
const float encoder_convertDeltaTimeToRPS = ENCODER_SAMPLESIZE * 1000000 / 45;
const float turnThreshold = 45.0;
const float tolerance = 1.0;
const float kpL = 0.8;
const float kpR = 0.8;
const float konstante = 0.04;
const float konstante_d = 1.35;

// Variables
volatile uint16_t encoder_counter[2];
volatile uint16_t encoder_totalImpulses[2];
volatile int8_t encoder_direction[2];
int encoder_drehzahl[2];
volatile uint32_t encoder_deltaTime[2];
volatile uint32_t encoder_oldTime[2];
volatile int16_t wantedRPSL;
volatile int16_t wantedRPSR;
float yaw = 0;
float deltaYaw = 0;
uint16_t turnCount = 0;
uint32_t lastTime = 0;
uint32_t prevTime = 0;
uint32_t actTime = 0;
int motor_speedLinks = 50;
int motor_speedRechts = 50;
int rps_start = 300;
int sum_Left = 0;
int sum_Right = 0;
int right_soll = 2500;
int left_soll = 2500;
int right_delta = 0;
int left_delta = 0;
int prev_right_delta = 0;
int prev_left_delta = 0;
uint16_t irSensor_Value[5];
uint16_t irSensor_offValue[5];
int irSensor_Delta[5];
bool irSensor_Side = true;  // true = rechte Wand
bool rechte_Seite = true;

const uint8_t irSensor_EnPins[] = {PIN_IRSENSOR_EN_L, PIN_IRSENSOR_EN_LV, PIN_IRSENSOR_EN_V, PIN_IRSENSOR_EN_RV,
                                   PIN_IRSENSOR_EN_R};
const uint8_t irSensor_InPins[] = {PIN_IRSENSOR_IN_L, PIN_IRSENSOR_IN_LV, PIN_IRSENSOR_IN_V, PIN_IRSENSOR_IN_RV,
                                   PIN_IRSENSOR_IN_R};

Servo myservo;
Adafruit_MPU6050 mpu;

// Turn sequences
const int direction1[] = {90, 90, 180, -180, -90, 90, 90, 90};
const int direction2[] = {-90, -90, -90, 90, 180, -180, -90, -90};
const int sequenceLength = 8;
int turnSequence[sequenceLength];
int turnIndex = 0;

// Function declarations
void ISC_Encoder_Rechts();
void ISC_Encoder_Links();
void encoder_calcDrehzahl();
void setMotor();
void updateSensValue();
void gyro();
bool checkDirection();

void setup() {
    DEBUG_SERIAL_BEGIN(9600);
    Wire.pins(I2C_SDA, I2C_SCL);
    Wire.begin();

    if (!mpu.begin()) {
        DEBUG_PRINTLN("Failed to find MPU6050 chip");
        while (1);
    }
    DEBUG_PRINTLN("MPU6050 Found!");

    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

    lastTime = millis();

    //myservo.attach(VAC_PIN);
    //myservo.write(5);

    pinMode(PIN_AKKU, INPUT);

    for (int i = 0; i < 5; i++) {
        pinMode(irSensor_EnPins[i], OUTPUT);
        pinMode(irSensor_InPins[i], INPUT);
    }

    attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LINKS_A), ISC_Encoder_Links, RISING);
    attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_RECHTS_A), ISC_Encoder_Rechts, RISING);

    digitalWrite(PIN_MOTOR_LINKS_DIR, LOW);
    digitalWrite(PIN_MOTOR_RECHTS_DIR, HIGH);
}

void loop() {
    //myservo.write(5);

    if (digitalReadFast(PIN_START) != LOW) return;
    //myservo.write(5);

    while (digitalReadFast(PIN_START) == LOW) {}

    analogWrite(PIN_MOTOR_LINKS_SPD, 20);
    analogWrite(PIN_MOTOR_RECHTS_SPD, 20);

    while (1) {
        prevTime = actTime;

        while (actTime - prevTime < 10) {
            actTime = millis();
        }

        updateSensValue();
        setMotor();
        gyro();
        DEBUG_PRINTLN(turnCount);

        if (!checkDirection()) {
            DEBUG_PRINTLN("Wrong direction!");
            // Handle wrong direction (e.g., stop the robot)
        }
    }
}

void readSensorValues() {
    for (int i = 0; i < 5; i++) {
        irSensor_offValue[i] = analogRead(irSensor_InPins[i]);
    }
}

void enableSensors() {
    for (int i = 0; i < 5; i++) {
        digitalWrite(irSensor_EnPins[i], HIGH);
    }
}

void disableSensors() {
    for (int i = 0; i < 5; i++) {
        digitalWrite(irSensor_EnPins[i], LOW);
    }
}

void calculateSensorValues() {
    delay(1);
    for (int i = 0; i < 5; i++) {
        irSensor_Value[i] = 1023 - (irSensor_offValue[i] - analogRead(irSensor_InPins[i]));
    }
}

void updateSensValue() {
    readSensorValues();
    enableSensors();
    calculateSensorValues();
    disableSensors();

    sum_Left = irSensor_Value[0] + irSensor_Value[1] + irSensor_Value[2];
    sum_Right = irSensor_Value[2] + irSensor_Value[3] + irSensor_Value[4];
    prev_right_delta = right_delta;
    prev_left_delta = left_delta;

    right_delta = right_soll - sum_Right;
    left_delta = left_soll - sum_Left;

    if (rechte_Seite) {
        int d_anteil = right_delta - prev_right_delta;

        int baseRPSL = 240;
        int baseRPSR = 240;

        wantedRPSL = baseRPSL - konstante * right_delta - d_anteil * konstante_d;
        wantedRPSR = baseRPSR + konstante * right_delta + d_anteil * konstante_d;

        if (irSensor_Value[4] > 960) {
            wantedRPSL = 260;
            wantedRPSR = 160;
        }

        if (irSensor_Value[3] > 965) {
            wantedRPSL = 290;
            wantedRPSR = 200;
        }

    } else {
        int d_anteil = left_delta - prev_left_delta;

        int baseRPSL = 240;
        int baseRPSR = 240;

        wantedRPSL = baseRPSL + konstante * left_delta + d_anteil * konstante_d;
        wantedRPSR = baseRPSR - konstante * left_delta - d_anteil * konstante_d;

        if (irSensor_Value[0] > 960) {
            wantedRPSL = 160;
            wantedRPSR = 260;
        }

        if (irSensor_Value[1] > 965) {
            wantedRPSL = 200;
            wantedRPSR = 290;
        }
    }
}

void setMotor() {
    encoder_calcDrehzahl();

    int errL = wantedRPSL - encoder_drehzahl[0];
    int errR = wantedRPSR - encoder_drehzahl[1];

    motor_speedLinks = (wantedRPSL / 3) + (kpL * errL);
    motor_speedRechts = (wantedRPSR / 3) + (kpR * errR);

    motor_speedLinks = constrain(motor_speedLinks, 0, MOTOR_MAXSPEED);
    motor_speedRechts = constrain(motor_speedRechts, 0, MOTOR_MAXSPEED);

    analogWrite(PIN_MOTOR_LINKS_SPD, motor_speedLinks);
    analogWrite(PIN_MOTOR_RECHTS_SPD, motor_speedRechts);
}

void encoder_calcDrehzahl() {
    if (encoder_deltaTime[0] > 0) encoder_drehzahl[0] = (1000000 / encoder_deltaTime[0]) * encoder_direction[0];
    if (encoder_deltaTime[1] > 0) encoder_drehzahl[1] = (1000000 / encoder_deltaTime[1]) * encoder_direction[1];

    if (micros() > (encoder_oldTime[0] + 500000)) {
        encoder_drehzahl[0] = 0;
        encoder_oldTime[0] = micros() - 500000;
    }

    if (micros() > (encoder_oldTime[1] + 500000)) {
        encoder_drehzahl[1] = 0;
        encoder_oldTime[1] = micros() - 500000;
    }
}

void ISC_Encoder_Links() {
    encoder_totalImpulses[0]++;
    encoder_direction[0] = digitalReadFast(PIN_ENCODER_LINKS_B) ? -1 : 1;
    encoder_counter[0] += encoder_direction[0];

    if (encoder_totalImpulses[0] & ENCODER_SAMPLESIZE == ENCODER_SAMPLESIZE) {
        unsigned long nowTime = micros();
        encoder_deltaTime[0] = nowTime - encoder_oldTime[0];
        encoder_oldTime[0] = nowTime;
    }
}

void ISC_Encoder_Rechts() {
    encoder_totalImpulses[1]++;
    encoder_direction[1] = digitalReadFast(PIN_ENCODER_RECHTS_B) ? 1 : -1;
    encoder_counter[1] += encoder_direction[1];

    if (encoder_totalImpulses[1] & ENCODER_SAMPLESIZE == ENCODER_SAMPLESIZE) {
        unsigned long nowTime = micros();
        encoder_deltaTime[1] = nowTime - encoder_oldTime[1];
        encoder_oldTime[1] = nowTime;
    }
}

void gyro() {
    unsigned long currentTime = millis();
    float dt = (currentTime - lastTime) / 1000.0;
    lastTime = currentTime;

    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    float gyroZ = g.gyro.z * 57.2958;

    deltaYaw = gyroZ * dt;
    yaw += deltaYaw;

    if (fabs(yaw) >= turnThreshold) {
        turnCount++;
        DEBUG_PRINT("Turn detected! Total turns: ");
        DEBUG_PRINTLN(turnCount);
        yaw = 0;

        // Record the turn
        if (turnIndex < sequenceLength) {
            turnSequence[turnIndex++] = (int)deltaYaw;
        }
    }
}

bool checkDirection() {
    for (int i = 0; i < turnIndex; i++) {
        if (turnSequence[i] != direction1[i] && turnSequence[i] != direction2[i]) {
            return false;
        }
    }
    return true;
}