package scanner.main

/**
 * Private property.
 * User: jIRKA
 * Date: 8.5.2015
 * Time: 16:48
 */
object Calculator {

  val laserPivotX = 90f
  val laserPivotZ = 50f
  val laserPivotZeroAngle = -40f

  def calculate(ptX: Int, ptY: Int, horizontalFov: Float, verticalFov: Float, width:Int, height: Int, laserAngle: Int, plateRotation: Float): (Float, Float, Float) = {
    val alpha = ptX * horizontalFov / width + 90 - horizontalFov / 2

    println(alpha)

    val alRad = Math.toRadians(alpha) // alpha radians
    val laRad = Math.toRadians(laserPivotZeroAngle - laserAngle) // laser radians

    val t = - (Math.sin(alRad) * laserPivotX - Math.cos(alRad) * laserPivotZ) / (- Math.cos(laRad) * Math.cos(alRad) + Math.sin(laRad) * Math.sin(alRad))

    println(t)

    val x3D = -(laserPivotX + Math.sin(laRad) * t).toFloat
    val z3D = (laserPivotZ + Math.cos(laRad) * t).toFloat

    val gamma = ptY * verticalFov / height - verticalFov / 2

    val y3D = -(Math.sin(Math.toRadians(gamma)) * Math.sqrt(x3D * x3D + z3D * z3D)).toFloat

    (x3D, y3D, z3D)

  }

}



val res = Calculator.calculate(340, 406, 54, 41, 648, 489, 2, 0)
  println("x: " + res._1)
  println("y: " + res._2)
  println("z: " + res._3)