//
//  Esp32ServoOutput.swift
//
//
//  Created by Michael Pivato on 9/9/20.
//

import Foundation
import SwiftyGPIO
import SwiftSerial

/**
 This class implements PWMOutput from the SwiftyGPIO package to communicate over Serial with an ESP32 that is running
 the sketch found in the esp32 subproject in picar: https://vato.ddns.net/gitlab/vato007/picar
 */
public class Esp32ServoOutput : PWMOutput {
    private let serialPort: SerialPort
    private let channel: UInt8
    private let pin: UInt8
    
    public static let ESP32_RESOLUTION: UInt8 = 8
    
    public init?(forChannel channel: UInt8, forPin pin: UInt8, onPort port: SerialPort) {
        if channel < 0 || channel > 16 {
            return nil
        }
        self.channel = channel
        // TODO: Add checks for pin number, or make init private with enums for each pin.
        self.pin = pin
        self.serialPort = port
    }
    
    public convenience init?(forChannel channel: UInt8, forPin pin: UInt8, onPort port: String) throws {
        let initPort = SerialPort(path: port)
        try initPort.openPort()
        initPort.setSettings(receiveRate: .baud115200, transmitRate: .baud115200, minimumBytesToRead: 1)
        self.init(forChannel: channel, forPin: pin, onPort: initPort)
    }
    
    public func initPWM() {
        do {
            let bytesWritten = try serialPort.writeData(Data([0, 1, channel, pin]))
            if bytesWritten != 4 {
                print("Wrote %d bytes, but should have written 4",  bytesWritten)
            }
            
        } catch {
            print("Failed to init PWM")
        }
    }
    
    public func startPWM(period ns: Int, duty percent: Float) {
        do {
            // Scale to 1 byte range, which is the current resolution.
            let bytesWritten = try serialPort.writeData(Data([channel, (UInt8)((percent / 100) * pow(2, Float(Esp32ServoOutput.ESP32_RESOLUTION)))]))
            if bytesWritten != 2 {
                print("Wrote %d bytes, but should have written 2", bytesWritten)
            }
        } catch {
            print("Failed to start PWM")
        }
    }
    
    public func stopPWM() {
        
    }
    
    // Patterns not supported for now.
    public func initPWMPattern(bytes count: Int, at frequency: Int, with resetDelay: Int, dutyzero: Int, dutyone: Int) {
        
    }
    
    public func sendDataWithPattern(values: [UInt8]) {
        
    }
    
    public func waitOnSendData() {
        
    }
    
    public func cleanupPattern() {
        
    }
    
    
}