#include "BiasModel.h"
#include <math.h>

BiasModel::BiasModel(int size, float distanceTolerance) {
    dSize = size;
    minPoints = size / 2;
    dTolerance = distanceTolerance * distanceTolerance;
    prevInsert = dPoints = level = 0;
    center = TNT::Vector<float>(3, 0.0);
    data = TNT::Array1D<TNT::Vector<float> >(size, TNT::Vector<float>(3, 0.0));
    coeff = TNT::Matrix<float>();
    _const = TNT::Vector<float>();
}

bool BiasModel::addPoint(TNT::Vector<float> &coord) {
    if (dPoints < dSize) {
        prevInsert = dPoints ? (prevInsert + 1) % dSize : 0;
        data[prevInsert] = coord;
        dPoints++;
    } else {
        int i;
        for (i = 0; i < dSize; i++) {
            TNT::Vector<float> vtmp = data[i] - coord;
            float distSqr = vtmp * vtmp;
            if (distSqr < dTolerance) {
                cerr << "already have " << dSize << " points and distance " << sqrt(distSqr) << " is below threshold " << sqrt(dTolerance) << endl;
                return false;
            }
        }
        prevInsert = dPoints ? (prevInsert + 1) % dSize : 0;
        data[prevInsert] = coord;
    }
    return true;
}

bool BiasModel::updateBias(float *input, float *output) {
    int i;
    TNT::Vector<float> coord = TNT::Vector<float>(3, input);
    coord -= center;
    if (addPoint(coord))
        minPoints--;
    if (!minPoints)
        minPoints = dSize / 2;
    else {
        output[0] = center[0]; output[1] = center[1]; output[2] = center[2];
        return false;
    }
    coeff.newsize(dPoints, 4);
    _const.newsize(dPoints);
    if (!level) {
        for (i = 0; i < dPoints; i++)
            center += data[i];
        for (i = 0; i < 3; i++)
            center[i] /= dPoints;
        level = 1;
        for (i = 0; i < dPoints; i++)
            data[i] -= center;
    }
    for (i = 0; i < dPoints; i++) {
        coeff[i][0] = 2 * data[i][0];
        coeff[i][1] = 2 * data[i][1];
        coeff[i][2] = 2 * data[i][2];
        coeff[i][3] = 1;
        _const[i] = data[i] * data[i];
    }
    TNT::Linear_Algebra::QR<float> decomp = TNT::Linear_Algebra::QR<float>(coeff);
    TNT::Vector<float> result = decomp.solve(_const);
    for (i = 0; i < 3; i++)
        result[3] += result[i] * result[i];
    result[3] = sqrt(result[3]);
    TNT::Vector<float> newOffset(3);
    newOffset[0] = result[0];
    newOffset[1] = result[1];
    newOffset[2] = result[2];
    for (i = 0; i < dPoints; i++) {
        data[i] -= newOffset;
    }
    center += newOffset;
    cout << "new estimate: <" << center[0] << "," << center[1] << "," << center[2] << "> radius " << result[3] << endl;
    output[0] = center[0]; output[1] = center[1]; output[2] = center[2];

}

int main(int argc, char *argv[]) {
    BiasModel bm = BiasModel(100, 0);
    float bias[3] = { 0.0, 0.0, 0.0 };
    while(!cin.eof()) {
        float tdata[3];
        cin >> tdata[0];
        cin >> tdata[1];
        cin >> tdata[2];
        cout << "read: " << tdata[0] << " " << tdata[1] << " " << tdata[2] << endl;
        bm.updateBias(tdata, bias);
        int i;
        for (i = 0; i < 3; i++)
            tdata[i] -= bias[i];
        cout << "filtered: " << tdata[0] << " " << tdata[1] << " " << tdata[2] << " (" << (sqrt(tdata[0]*tdata[0]+tdata[1]*tdata[1]+tdata[2]*tdata[2])) << ")" << endl;
    }
    return 0;
}