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

import Foundation
import SwiftyGPIO


protocol Vehicle2D{
    var throttle: Float {get set}
    var steering: Float {get set}
}

class MockVehicle: Vehicle2D {
    var throttle: Float = 0
    var steering: Float = 0
}

public class RPiVehicle2D: Vehicle2D{
    
    public var pwmThrottle: Servo
    public var pwmSteering: Servo
    
    var throttle: Float{
        get{
            return pwmThrottle.value
        }
        set(value){
            pwmThrottle.value = value
        }
    }
    
    var steering: Float{
        get{
            return pwmSteering.value
        }
        set(value){
            pwmSteering.value = value
        }
    }
    
    init(withThrottlePin: Servo, withSteeringPin: Servo){
        pwmThrottle = withThrottlePin
        pwmSteering = withSteeringPin
    }
    
    
    func stop(){
        pwmThrottle.detach()
        pwmSteering.detach()
    }
    
}

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
    }
}