//
//  Servo.swift
//
//  Simple wrappers for PwmOutput I'll create as I go along, to match gpioZero's implementation I'm currently using.
//  See gpiozero code here: https://github.com/gpiozero/gpiozero/blob/master/gpiozero/output_devices.py
//
//  Created by Michael Pivato on 9/5/20.
//

import Foundation
import SwiftyGPIO

enum ServoError: Error{
    case invalidMinPulseWidth
    case invalidMaxPulseWidth
}

protocol Servo {
    var value: Float {get set}
    
    var minPulseWidth: Float {get}
    var maxPulseWidth: Float {get}
    var pulseWidth: Float {get}
    
    func min()
    func mid()
    func max()
    func detach()
}

public class PWMHardwareServo : Servo{
    private (set) var frameWidth : Float
    private var minDc: Float
    private var dcRange: Float
    private var minValue: Float
    private var valueRange: Float
    private var internalValue: Float
    
    // TODO: Use a factory to provide the correct pwm rather than passing the pin in.
    private var device: PWMOutput
    private var currentPulseWIdth: Float
    
    var minPulseWidth: Float {
        get{
            return minDc * frameWidth
        }
    }
    
    var maxPulseWidth: Float{
        get{
            return (dcRange * frameWidth) + minPulseWidth
        }
    }
    
    var pulseWidth: Float{
        get{
            return internalValue * frameWidth
        }
    }
    
    var value: Float{
        get{
            internalValue
        }
        set(newValue){
            internalValue = newValue
            device.startPWM(period: Int(frameWidth),
                            // Multiply by 100 to get into percentage. I don't like that...
                duty: minDc + dcRange * ((internalValue - minValue) / valueRange) * 100)
        }
    }

    
    init?(forPin pin: PWMOutput, startingAt initialValue: Float = 0, withMinPulseWidth minPulseWidth: Float = 1000000,
          withMaxPulseWidth maxPulseWidth: Float = 2000000, withFrameWidth frameWidth: Float = 20000000) throws {
        if(minPulseWidth >= maxPulseWidth){
            throw ServoError.invalidMinPulseWidth
        }
        
        if(maxPulseWidth >= frameWidth){
            throw ServoError.invalidMaxPulseWidth
        }
        self.frameWidth = frameWidth
        self.minDc = minPulseWidth / frameWidth
        self.dcRange = (maxPulseWidth - minPulseWidth) / frameWidth
        self.minValue = -1
        self.valueRange = 2
        
        self.currentPulseWIdth = 0
        
        // Initialise pin immediately.
        self.device = pin
        self.device.initPWM()
        self.internalValue = initialValue
        self.device.startPWM(period: Int(self.frameWidth), duty: self.minDc + self.dcRange * ((internalValue - self.minValue) / self.valueRange) * 100)
    }
    
    func min(){
        self.value = -1
    }
    
    func mid(){
        self.value = 0
    }
    
    func max() {
        self.value = 1
    }
    
    func detach(){
        self.device.stopPWM()
    }
}