//
//  Vehicle.swift
//
//
//  Created by Michael Pivato on 8/5/20.
//

import Foundation
import Swift2dCar

public typealias ThrottleHandler = (_ magnitude: Float) -> Float
public typealias SteeringHandler = (_ angle: Float) -> Float


public class IntelligentPiCar : RPiVehicle2D {
    
    private var vehicleLength: Float?
    private var throttleFunc: ThrottleHandler?
    private var steeringFunc: SteeringHandler?
    
    /**:
     Calibration function for how the car moves (acoording to a bicycle model) for a given throttle/steering angle. This sets the way the
     - Parameters
        - carLength
     */
    func calibrate(vehicleLength: Float, throttleFunc: @escaping ThrottleHandler, steeringFunc: @escaping SteeringHandler){
        // Define a function that indicates how the throttle/steering should be set for given magnitude/angle to move.
        self.vehicleLength = vehicleLength
        self.throttleFunc = throttleFunc
        self.steeringFunc = steeringFunc
    }
    
    /**
     Move the car by the given magnitude and angle, depending on how the is known to move with apriori throttle/steering.
     */
    func move2D(magnitude: Float, angle: Float) {
        if let throttleFunc = self.throttleFunc {
//            self.pwmThrottle.value = throttleFunc(magnitude)
        }
        
        if let steeringFunc = steeringFunc {
//            self.pwmSteering.value = steeringFunc(angle)
        }
    }
    
    /**
     Move to the coordinates relative to the car. You must first calibrate the car. A bicycle model is assumed.
     */
    func moveRelativeTo2D(x: Float, y: Float) {
        // TODO: This function, has a lot of edge cases. Also is really
    }
}