/**
 * @file calibrateCompass.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief 
 * @version 0.1
 * @date 2023-05-23
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "calibrateCompass.h"

CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) {
    this->compass = compass;
    this->state = State::Ready;
    this->clearData();
}

void CalibrateCompass::loop() {
    if (this->state != State::Calibrating)
        return;

    bool changed = false;

    this->compass->read();
    int x = this->compass->getX();
    int y = this->compass->getY();
    int z = this->compass->getZ();

    if(x < this->data.data[0][0]) {
        this->data.data[0][0] = x;
        changed = true;
    }

    if(x > this->data.data[0][1]) {
        this->data.data[0][1] = x;
        changed = true;
    }

    if(y < this->data.data[1][0]) {
        this->data.data[1][0] = y;
        changed = true;
    }

    if(y > this->data.data[1][1]) {
        this->data.data[1][1] = y;
        changed = true;
    }

    if(z < this->data.data[2][0]) {
        this->data.data[2][0] = z;
        changed = true;
    }

    if(z > this->data.data[2][1]) {
        this->data.data[2][1] = z;
        changed = true;
    }

    if (changed)
        this->lastChange = millis();

    if (millis() - this->lastChange > this->maxTimeWithoutChange)
        this->state = State::Finished;
}

void CalibrateCompass::start() {
    if (this->state != State::Ready)
        return;

    this->clearData();
    this->state = State::Calibrating;
    this->compass->setCalibration(0, 0, 0, 0, 0, 0);
    this->lastChange = millis();
}

void CalibrateCompass::useData() {
    if (this->state != State::Finished)
        return;
    
    this->compass->setCalibration(  this->data.data[0][0],
                                    this->data.data[0][1],
                                    this->data.data[1][0],
                                    this->data.data[1][1],
                                    this->data.data[2][0],
                                    this->data.data[2][1]
                                    )
}

void CalibrateCompass::resetData() {
    this->reset();
    this->compass->setCalibration(0, 0, 0, 0, 0, 0);
}

void CalibrateCompass::reset() {
    this->clearData();
    this->state = State::Ready;
}

void CalibrateCompass::clearData() {
    for (uint8_t i = 0; i < 3; i++) {
        this->data.data[i][0] = 0;
        this->data.data[i][1] = 0;
    }
}