#include "prg_342.h"

volatile int PRG_342::encoderPos = 0;  // Initialisierung der statischen Variablen
volatile int PRG_342::lastEncoded = 0; // Initialisierung der statischen Variablen
volatile int PRG_342::ANGLE = 0;   // Initialisierung der statischen Variablen
const int PRG_342::EncoderPinA;         // Definition der statischen Variablen
const int PRG_342::EncoderPinB;         // Definition der statischen Variablen
const int PRG_342::EncoderPinN;         // Definition der statischen Variablen

const int PRG_342::DMS_PIN;         // Definition der statischen Variablen
const int PRG_342::EXTSET_PIN;         // Definition der statischen Variablen
const int PRG_342::TORQUE_OUT_PIN;         // Definition der statischen Variablen
const int PRG_342::ACT_OUT_PIN;         // Definition der statischen Variablen
int PRG_342::valA0;         // Definition der statischen Variablen
int PRG_342::valA1;         // Definition der statischen Variablen
int PRG_342::filteredValA1;

volatile bool PRG_342::isRunning;

PRG_342::PRG_342(){
    this->valA0 = 0;
    this->valA1 = 0;
    this->filteredValA1 = 0;
    this->TORQUE = 0;
    this->OFFSET = 0;
    this->SETPOINT = 0;
    this->ANGLE = 0;
    this->K_FAKTOR = 1.0;

    lastSpeedCalcTime = millis();
    lastAngle = this->getAngle();

    analogWrite(TORQUE_OUT_PIN, 0);
    analogWrite(ACT_OUT_PIN, 0);
    analogWrite(DMS_PIN, 0);
    analogWrite(EXTSET_PIN, 0);
    digitalWrite(EncoderPinA, LOW);
    digitalWrite(EncoderPinB, LOW);

    pinMode(DMS_PIN, INPUT);
    pinMode(EXTSET_PIN, INPUT);
    pinMode(EncoderPinA, INPUT);
    pinMode(EncoderPinB, INPUT);
    pinMode(TORQUE_OUT_PIN, OUTPUT);
    pinMode(ACT_OUT_PIN, OUTPUT);
    
    analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
    analogReadResolution(12);

    attachInterrupt(digitalPinToInterrupt(EncoderPinA), PRG_342::updateEncoder, CHANGE);
    attachInterrupt(digitalPinToInterrupt(EncoderPinB), PRG_342::updateEncoder, CHANGE);
}

PRG_342::PRG_342(float K_FAKTOR) {
    this->valA0 = 0;
    this->valA1 = 0;
    this->TORQUE = 0;
    this->OFFSET = 0;
    this->SETPOINT = 0;
    this->ANGLE = 0;
    this->K_FAKTOR = K_FAKTOR;

    lastSpeedCalcTime = millis();
    lastAngle = this->getAngle();

    analogWrite(TORQUE_OUT_PIN, 0);
    analogWrite(ACT_OUT_PIN, 0);
    analogWrite(DMS_PIN, 0);
    analogWrite(EXTSET_PIN, 0);
    digitalWrite(EncoderPinA, LOW);
    digitalWrite(EncoderPinB, LOW);

    pinMode(DMS_PIN, INPUT);
    pinMode(EXTSET_PIN, INPUT);
    pinMode(EncoderPinA, INPUT);
    pinMode(EncoderPinB, INPUT);
    pinMode(TORQUE_OUT_PIN, OUTPUT);
    pinMode(ACT_OUT_PIN, OUTPUT);
    
    analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
    analogReadResolution(12);

    attachInterrupt(digitalPinToInterrupt(EncoderPinA), PRG_342::updateEncoder, CHANGE);
    attachInterrupt(digitalPinToInterrupt(EncoderPinB), PRG_342::updateEncoder, CHANGE);
}

PRG_342::~PRG_342() {
    detachInterrupt(digitalPinToInterrupt(EncoderPinA));
    detachInterrupt(digitalPinToInterrupt(EncoderPinB));
}

void PRG_342::updateValA0(){
    this->valA0 = analogRead(DMS_PIN);
}

void PRG_342::updateValA1(){
    this->valA1 = analogRead(EXTSET_PIN);
    this->filteredValA1 = this->exponentialFilter(this->valA1, this->filteredValA1, 0.01);
}

float PRG_342::exponentialFilter(float currentValue, float previousFilteredValue, float alpha) {
  return alpha * currentValue + (1.0 - alpha) * previousFilteredValue;
}

/*void PRG_342::setOutput(unsigned int value){
    if(value <= 0){
        value = 50;
    } else if (value > 4095){
        value = 4095;
    }

    analogWrite(TORQUE_OUT_PIN, value);
}*/

/*void PRG_342::setOutput(unsigned int value, bool noLimits, bool onlyWhenRunning){
    if(value <= 0 && !noLimits){
        value = 50;
    } else if (value > 4095 && !noLimits){
        value = 4095;
    }

    if(onlyWhenRunning){
        if(this->isRunning){
            analogWrite(TORQUE_OUT_PIN, value);
        }
    } else {
        analogWrite(TORQUE_OUT_PIN, value);
    }
    this->lastOutput = value;
}*/

void PRG_342::setOutput(unsigned int value, bool onlyWhenRunning){
    value = constrain(value, 0, 4095);
    if(onlyWhenRunning){
        if(this->isRunning){
            if(abs(this->getRotationSpeed()) >= 400){
                analogWrite(TORQUE_OUT_PIN, value);
                this->ActOut = value;
            }
        }
    } else {
        analogWrite(TORQUE_OUT_PIN, value);
        this->ActOut = value;
    }
}

void PRG_342::setOutput(int startValue, int endValue, int max_mV_per_ms){
    endValue = constrain(endValue, 0, 4095);
    int actValue = startValue;
    int changeValue;
    while(abs(actValue - endValue) > max_mV_per_ms + 1){
        changeValue = endValue - startValue;
        changeValue = constrain(changeValue, max_mV_per_ms * -1, max_mV_per_ms);
        actValue += changeValue;
        delayMicroseconds(1000);
    }
    analogWrite(TORQUE_OUT_PIN, actValue);
    this->ActOut = actValue;
}

void PRG_342::updateActOut(int actTorque){
    analogWrite(ACT_OUT_PIN, abs(actTorque) * 0.0819);
}

int PRG_342::getActTorque(){
    this->updateValA0();
    return ((this->valA0 * 24.4200244) - 50000) * this->K_FAKTOR;
    //return this->valA0;
}

unsigned int PRG_342::getSetPoint(unsigned char format){
    this->updateValA1();
    if(format == 0){
        return this->filteredValA1;
    } else if (format == 1){
        return (this->filteredValA1 * 2.44200244);
    }
}

unsigned int PRG_342::getOutVolt(){
    return this->ActOut * 6.34920635;
}

void PRG_342::tareTorque(){
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->TORQUE = this->getTorque(false, 0.01);
    this->OFFSET = 0 - this->TORQUE; 
}

void PRG_342::tareAngle(){
    this->ANGLE = 0;
    this->lastEncoded = 0;
    this->encoderPos = 0;
}

void PRG_342::updateActTorqueOut(){
    this->TORQUE = this->getActTorque();
    analogWrite(this->TORQUE_OUT_PIN, abs(this->TORQUE + this->OFFSET) * 0.0819);
}

void PRG_342::updateEncoder(){
    int MSB = digitalRead(EncoderPinA);  // Most Significant Bit
    int LSB = digitalRead(EncoderPinB);  // Least Significant Bit

    int encoded = (MSB << 1) | LSB;          // Kombinieren der beiden Bits
    int sum = (PRG_342::lastEncoded << 2) | encoded;  // Kombinieren mit dem letzten Zustand

    // Bestimmen der Richtungsänderung und Aktualisierung der Position
    if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) PRG_342::encoderPos++;
    if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) PRG_342::encoderPos--;

    PRG_342::lastEncoded = encoded;  // Aktualisieren des letzten Zustands

    PRG_342::ANGLE = PRG_342::encoderPos * 18;

    isRunning = true;
}

int PRG_342::getTorque(int offset){
    if(offset){
        return this->getActTorque() + this->OFFSET; 
    } else {
        return this->getActTorque();
    }
}

/*int PRG_342::getTorque(int offset, unsigned int COUNTS){
    this->TORQUE = this->getActTorque();
    for (int i = 0; i < COUNTS-1; i++) {
        this->TORQUE += this->getActTorque();
    }
    this->TORQUE = (float)this->TORQUE / COUNTS;
    if(offset){
        return this->TORQUE + this->OFFSET; 
    } else {
        return this->TORQUE;
    }
}*/

int PRG_342::getTorque(int offset, float ALPHA){
    this->TORQUE = ALPHA * (float)this->getActTorque() + (1.0 - ALPHA) * (float)this->TORQUE;
    if(offset){
        return this->TORQUE + this->OFFSET; 
    } else {
        return this->TORQUE;
    }
}

int PRG_342::getAngle(){
    //this->updateEncoder();
    return this->ANGLE;
}

void PRG_342::safeShutdown(){
    this->setOutput(4095);
    for (int i = 0; i < 4065; i++){
        this->setOutput(4095 - i);
        delay(10);
    }
}

void PRG_342::safeShutdown(unsigned int time){
    unsigned int DELAY = time * 244.200244;
    this->setOutput(4095);
    for (int i = 0; i < 4065; i++){
        this->setOutput(4095 - i);
        delayMicroseconds(DELAY);
    }
}

void PRG_342::safeShutdown(unsigned int time, unsigned int value){
    unsigned int DELAY = time * 244.200244;
    value += 100;
    this->setOutput(value);
    for (int i = 0; i < value + 1; i++){
        this->setOutput(value - i);
        delayMicroseconds(DELAY);
    }
}

void PRG_342::DynDecog(unsigned int actValue){
    this->setOutput(4095 * 0.5);
    delay(500);
    this->setOutput(4095 * 0.1);
    delay(500);
    this->setOutput(4095 * 0.5);
    delay(500);
    this->setOutput(actValue);
}

float PRG_342::getRotationSpeed() {
    // Aktuelle Zeit und Winkel holen
    unsigned long currentTime = millis();
    int currentAngle = this->getAngle();

    // Zeitdifferenz und Winkeldifferenz berechnen
    unsigned long timeDelta = currentTime - lastSpeedCalcTime;
    int angleDelta = currentAngle - lastAngle;

    // Aktualisieren der letzten Winkel- und Zeitwerte
    lastSpeedCalcTime = currentTime;
    lastAngle = currentAngle;

    // Verhindern einer Division durch Null
    if (timeDelta == 0) return 0;

    // Drehgeschwindigkeit in Grad pro Sekunde berechnen
    float speed = (angleDelta) * (1000.0 / timeDelta);

    return speed;
}

void PRG_342::AutoDecog(){
    for(float k = 10; k > 0; k--){
        for(int j = 0; j < 100; j++){
            float samples = 1000.0;
            for (float i = 0; i < samples; i++){
                analogWrite(TORQUE_OUT_PIN, sin(2.0 * PI * (i / samples)) * 4095.0 * 0.5 * (k / 100) + 4095 * 0.5 );
            }
        }
    }

    /*this->setOutput(4095 * 0);
    delay(100);
    for(float k = 0; k < 0.5; k = k + 0.05){
        this->setOutput(4095 * k);
        while(this->getRotationSpeed() < 400){}
        delay(100);
        Serial.print("output: ");
        Serial.println(4095 * k);
        Serial.print("isRunning: ");
        Serial.println(isRunning);
        Serial.print("getRotationSpeed(): ");
        Serial.println(getRotationSpeed());
    }
    delay(100);
    for(float i = 0.5; i > 0.0; i = i - 0.05){
        this->setOutput(4095 * i);
        while(this->getRotationSpeed() < 400){}
        delay(100);
        Serial.print("output: ");
        Serial.println(4095 * i);
        Serial.print("isRunning: ");
        Serial.println(isRunning);
        Serial.print("getRotationSpeed(): ");
        Serial.println(getRotationSpeed());
    }
    Serial.println("AutoDecog ended.");*/
}