import 'dart:math';
import 'package:simple_kalman/simple_kalman.dart';



class CompassUpdater {
  double? _lastAngle;
  var _angle = 0.0;
  final _kalman = SimpleKalman(errorMeasure: 256, errorEstimate: 150, q: 0.9);

  double angleDis(double lastAngle, double nowAngle){


    var tmp = lastAngle;
    var lastDis = nowAngle - tmp;
    while(true){
      tmp += 360;
      var dis = nowAngle - tmp;
      if(lastDis.abs() < dis.abs()){
        break;
      }
      lastDis = dis;
      lastAngle=tmp;
    }
    tmp = lastAngle;
    lastDis = nowAngle - tmp;
    while(true){
      tmp -= 360;
      var dis = nowAngle - tmp;
      if(lastDis.abs() < dis.abs()){
        break;
      }
      lastDis = dis;
      lastAngle=tmp;
    }

    return nowAngle - lastAngle;
  }



  double? updateRadians(double direction){
    var nowAngle = direction * 180 / pi;
    var lastAngle = _lastAngle;
    if (lastAngle != null) {
      _lastAngle = nowAngle;

      var da = angleDis(lastAngle, nowAngle);

      _angle += da;
      return _kalman.filtered(_angle) * pi / 180.0;
    } else {
      _angle = nowAngle;
      _lastAngle = nowAngle;
      for(var i =0; i< 50; i++){
        _kalman.filtered(_angle);
      }
      return null;
    }
  }

}