VFH 90% done. Parameter need to fix and small bug