import Foundation
import CoreMotion

struct SimpleMovingAvgQueue{
    typealias EulerAngles = (roll: Double, pitch: Double, yaw: Double)
    var eulerAngles = Array<EulerAngles>()
    let capacity: Int
    
    init(capacity cap: Int) {
        self.capacity = cap
    }
    
    mutating func push(deviceMotion: CMDeviceMotion) -> (roll: Double, pitch: Double, yaw: Double )?{
        return self.push(roll: deviceMotion.attitude.roll, pitch: deviceMotion.attitude.pitch, yaw: deviceMotion.attitude.yaw)
    }
    
    mutating func push(roll: Double, pitch: Double, yaw: Double ) -> (roll: Double, pitch: Double, yaw: Double )? {
        eulerAngles.append((roll, pitch, yaw))
        
        guard eulerAngles.count >= capacity else {
            return nil
        }
        
        if eulerAngles.count > capacity {
            eulerAngles.remove(at: 0)
        }
        
        let sum = self.eulerAngles.reduce((0.0, 0.0, 0.0)) {
            (v1, v2) -> (roll: Double, pitch: Double, yaw: Double) in
            return (v1.roll+v2.roll,
                    v1.pitch+v2.pitch,
                    v1.yaw+v2.yaw)
        }
        
        //平均値求め、radianから角度に変換
        return convertRadianToAngle(roll:sum.roll/Double(capacity),
                                    pitch:sum.pitch/Double(capacity),
                                    yaw:sum.yaw/Double(capacity))
    }
    
    func convertRadianToAngle(roll: Double, pitch: Double, yaw: Double) -> (roll: Double, pitch: Double, yaw: Double) {
        return (self.convertRadianToAngle(radian: roll),
                self.convertRadianToAngle(radian: pitch),
                self.convertRadianToAngle(radian: yaw))
    }
    
    func convertRadianToAngle(radian: Double) -> Double {
        return radian * 180 / M_PI
    }
}