VERSION 5.00
Begin VB.Form frmMain 
   Caption         =   "Image Demo"
   ClientHeight    =   10995
   ClientLeft      =   60
   ClientTop       =   345
   ClientWidth     =   10710
   LinkTopic       =   "Form1"
   ScaleHeight     =   549.75
   ScaleMode       =   2  'Point
   ScaleWidth      =   535.5
   StartUpPosition =   3  'Windows Default
   Begin VB.TextBox txtCurPosition 
      Height          =   315
      Left            =   7380
      TabIndex        =   31
      Text            =   "-1"
      Top             =   7620
      Width           =   1035
   End
   Begin VB.TextBox txtCurColor 
      Height          =   315
      Left            =   7380
      TabIndex        =   26
      Text            =   "-1"
      Top             =   7980
      Width           =   855
   End
   Begin VB.TextBox txtDistance 
      Height          =   285
      Left            =   4200
      TabIndex        =   17
      Text            =   "-1"
      Top             =   9060
      Width           =   615
   End
   Begin VB.TextBox txtLaserAngleZ 
      Height          =   315
      Left            =   4200
      TabIndex        =   15
      Text            =   "-1"
      Top             =   7980
      Width           =   615
   End
   Begin VB.TextBox txtScanAngleZ 
      Height          =   285
      Left            =   4200
      TabIndex        =   14
      Text            =   "-1"
      Top             =   8700
      Width           =   615
   End
   Begin VB.TextBox txtScanAngleX 
      Height          =   285
      Left            =   4200
      TabIndex        =   12
      Text            =   "-1"
      Top             =   8340
      Width           =   615
   End
   Begin VB.TextBox txtDistBetween 
      Height          =   285
      Left            =   1380
      TabIndex        =   8
      Text            =   "54.5"
      Top             =   8340
      Width           =   615
   End
   Begin VB.TextBox txtLaserAngleX 
      Height          =   315
      Left            =   4200
      TabIndex        =   7
      Text            =   "-1"
      Top             =   7620
      Width           =   615
   End
   Begin VB.TextBox txtFOVLaser 
      Height          =   285
      Left            =   1380
      TabIndex        =   5
      Text            =   "180"
      Top             =   7980
      Width           =   615
   End
   Begin VB.CommandButton cmdStep 
      Caption         =   "Step"
      Height          =   315
      Left            =   9780
      TabIndex        =   1
      Top             =   480
      Width           =   495
   End
   Begin VB.CommandButton cmdStart 
      Caption         =   "Start"
      Height          =   315
      Left            =   9780
      TabIndex        =   0
      Top             =   120
      Width           =   495
   End
   Begin VB.TextBox txtFOVCamera 
      Height          =   315
      Left            =   1380
      TabIndex        =   2
      Text            =   "60"
      Top             =   7620
      Width           =   615
   End
   Begin VB.Timer tmrMain 
      Enabled         =   0   'False
      Interval        =   1
      Left            =   9840
      Top             =   840
   End
   Begin VB.PictureBox imgInput 
      AutoSize        =   -1  'True
      Height          =   7260
      Left            =   120
      Picture         =   "frmImageDemo.frx":0000
      ScaleHeight     =   480
      ScaleMode       =   3  'Pixel
      ScaleWidth      =   640
      TabIndex        =   28
      TabStop         =   0   'False
      Top             =   120
      Width           =   9660
      Begin VB.Shape shpPos 
         BorderColor     =   &H00FF0000&
         Height          =   135
         Left            =   2400
         Top             =   3120
         Width           =   135
      End
      Begin VB.Line linePos 
         BorderColor     =   &H00FF0000&
         X1              =   0
         X2              =   160
         Y1              =   8
         Y2              =   208
      End
      Begin VB.Shape shpCurrent 
         FillColor       =   &H0000FFFF&
         FillStyle       =   0  'Solid
         Height          =   135
         Left            =   900
         Shape           =   4  'Rounded Rectangle
         Top             =   840
         Width           =   135
      End
      Begin VB.Shape testdot 
         FillColor       =   &H000000FF&
         FillStyle       =   0  'Solid
         Height          =   195
         Index           =   0
         Left            =   3120
         Shape           =   3  'Circle
         Tag             =   "125.92"
         Top             =   3420
         Width           =   195
      End
      Begin VB.Shape testdot 
         FillColor       =   &H000000FF&
         FillStyle       =   0  'Solid
         Height          =   195
         Index           =   2
         Left            =   1320
         Shape           =   3  'Circle
         Tag             =   "125"
         Top             =   60
         Width           =   195
      End
      Begin VB.Shape testdot 
         FillColor       =   &H000000FF&
         FillStyle       =   0  'Solid
         Height          =   195
         Index           =   1
         Left            =   1140
         Shape           =   3  'Circle
         Tag             =   "97.69"
         Top             =   3420
         Width           =   195
      End
      Begin VB.Shape testdot 
         FillColor       =   &H000000FF&
         FillStyle       =   0  'Solid
         Height          =   195
         Index           =   3
         Left            =   7860
         Shape           =   3  'Circle
         Tag             =   "130"
         Top             =   3240
         Width           =   195
      End
      Begin VB.Line Line9 
         BorderColor     =   &H0000FF00&
         X1              =   320
         X2              =   636
         Y1              =   476
         Y2              =   236
      End
      Begin VB.Line Line8 
         BorderColor     =   &H0000FF00&
         X1              =   0
         X2              =   320
         Y1              =   236
         Y2              =   476
      End
      Begin VB.Label Label1 
         BackStyle       =   0  'Transparent
         Caption         =   "FOV"
         ForeColor       =   &H0000FF00&
         Height          =   255
         Left            =   5100
         TabIndex        =   30
         Top             =   6960
         Width           =   375
      End
      Begin VB.Line Line10 
         BorderColor     =   &H0000FF00&
         X1              =   320
         X2              =   332
         Y1              =   456
         Y2              =   468
      End
      Begin VB.Label Label2 
         BackStyle       =   0  'Transparent
         Caption         =   "A"
         ForeColor       =   &H0000FF00&
         Height          =   255
         Left            =   4440
         TabIndex        =   29
         Top             =   6660
         Width           =   255
      End
      Begin VB.Line Line11 
         BorderColor     =   &H0000FF00&
         BorderStyle     =   3  'Dot
         X1              =   204
         X2              =   324
         Y1              =   236
         Y2              =   484
      End
      Begin VB.Line Line14 
         BorderColor     =   &H0000FF00&
         X1              =   320
         X2              =   308
         Y1              =   456
         Y2              =   468
      End
      Begin VB.Line lineAlign2 
         BorderColor     =   &H00800000&
         X1              =   0
         X2              =   636
         Y1              =   476
         Y2              =   0
      End
      Begin VB.Line LineAlign3 
         BorderColor     =   &H00800000&
         X1              =   636
         X2              =   0
         Y1              =   236
         Y2              =   236
      End
      Begin VB.Line LineAlign4 
         BorderColor     =   &H00800000&
         X1              =   320
         X2              =   320
         Y1              =   476
         Y2              =   0
      End
      Begin VB.Line lineAlign1 
         BorderColor     =   &H00800000&
         X1              =   636
         X2              =   0
         Y1              =   476
         Y2              =   0
      End
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   8
      Left            =   8580
      Top             =   8280
      Width           =   135
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   6
      Left            =   8280
      Top             =   8280
      Width           =   135
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   3
      Left            =   8580
      Top             =   7980
      Width           =   135
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   195
      Index           =   5
      Left            =   8580
      Top             =   8100
      Width           =   135
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   7
      Left            =   8400
      Top             =   8280
      Width           =   195
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   195
      Index           =   0
      Left            =   8400
      Top             =   8100
      Width           =   195
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   195
      Index           =   4
      Left            =   8280
      Top             =   8100
      Width           =   135
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   2
      Left            =   8400
      Top             =   7980
      Width           =   195
   End
   Begin VB.Shape shpColor 
      FillColor       =   &H00FFFFFF&
      FillStyle       =   0  'Solid
      Height          =   135
      Index           =   1
      Left            =   8280
      Top             =   7980
      Width           =   135
   End
   Begin VB.Label Label19 
      Caption         =   "Position"
      Height          =   195
      Left            =   6780
      TabIndex        =   32
      Top             =   7680
      Width           =   615
   End
   Begin VB.Label Label18 
      Caption         =   "Color"
      Height          =   195
      Left            =   6780
      TabIndex        =   27
      Top             =   8040
      Width           =   615
   End
   Begin VB.Label Label16 
      Caption         =   "o"
      Height          =   195
      Left            =   2040
      TabIndex        =   24
      Top             =   7920
      Width           =   255
   End
   Begin VB.Label Label14 
      Caption         =   "o"
      Height          =   195
      Left            =   4860
      TabIndex        =   22
      Top             =   7920
      Width           =   255
   End
   Begin VB.Label Label13 
      Caption         =   "o"
      Height          =   195
      Left            =   4860
      TabIndex        =   21
      Top             =   8280
      Width           =   255
   End
   Begin VB.Label Label12 
      Caption         =   "o"
      Height          =   195
      Left            =   4860
      TabIndex        =   20
      Top             =   8640
      Width           =   255
   End
   Begin VB.Label Label11 
      Caption         =   """"
      Height          =   195
      Left            =   4860
      TabIndex        =   19
      Top             =   9120
      Width           =   255
   End
   Begin VB.Label lbldistance 
      Caption         =   "Distance"
      Height          =   195
      Left            =   3120
      TabIndex        =   18
      Top             =   9120
      Width           =   975
   End
   Begin VB.Label Label10 
      Caption         =   "Laser Angle Z"
      Height          =   195
      Left            =   3120
      TabIndex        =   16
      Top             =   8040
      Width           =   1095
   End
   Begin VB.Label Label8 
      Caption         =   "Scan Angle Z"
      Height          =   195
      Left            =   3120
      TabIndex        =   13
      Top             =   8760
      Width           =   975
   End
   Begin VB.Label Label7 
      Caption         =   "Scan Angle X"
      Height          =   195
      Left            =   3120
      TabIndex        =   11
      Top             =   8400
      Width           =   975
   End
   Begin VB.Label Label6 
      Caption         =   """"
      Height          =   195
      Left            =   2040
      TabIndex        =   10
      Top             =   8400
      Width           =   255
   End
   Begin VB.Label Label5 
      Caption         =   "Dist Between"
      Height          =   195
      Left            =   300
      TabIndex        =   9
      Top             =   8340
      Width           =   1035
   End
   Begin VB.Label Label4 
      Caption         =   "Laser Angle X"
      Height          =   195
      Left            =   3120
      TabIndex        =   6
      Top             =   7680
      Width           =   1095
   End
   Begin VB.Label Label3 
      Caption         =   "Laser FOV"
      Height          =   195
      Left            =   300
      TabIndex        =   4
      Top             =   7980
      Width           =   1035
   End
   Begin VB.Line Line1 
      BorderColor     =   &H000000FF&
      BorderStyle     =   0  'Transparent
      Index           =   0
      X1              =   255
      X2              =   81
      Y1              =   480
      Y2              =   255
   End
   Begin VB.Label Label9 
      Caption         =   "Camera FOV"
      Height          =   195
      Left            =   300
      TabIndex        =   3
      Top             =   7680
      Width           =   975
   End
   Begin VB.Label Label17 
      Caption         =   "o"
      Height          =   195
      Left            =   2040
      TabIndex        =   25
      Top             =   7560
      Width           =   255
   End
   Begin VB.Label Label15 
      Caption         =   "o"
      Height          =   195
      Left            =   4860
      TabIndex        =   23
      Top             =   7560
      Width           =   255
   End
End
Attribute VB_Name = "frmMain"
Attribute VB_GlobalNameSpace = False
Attribute VB_Creatable = False
Attribute VB_PredeclaredId = True
Attribute VB_Exposed = False
Option Explicit
Private Declare Function GetPixel Lib "gdi32" (ByVal hDC As Long, ByVal x As Long, ByVal y As Long) As Long

Dim cur_laser As Integer

Dim cur_x As Integer
Dim cur_y As Integer
Dim cur_w As Integer
Dim cur_h As Integer
Dim step_x As Integer
Dim step_y As Integer
Dim padding As Integer

Private Sub Form_Load()
   Dim dot As Shape
   
   padding = 1 ' padding around edges, for visual representations
   
   cur_x = padding ' offset for padding
   cur_y = padding
   step_x = 1 ' pixels (scalemode=pixels)
   step_y = 1
   
   linePos.X1 = 0
   linePos.Y1 = 0
   shpPos.Width = padding * 4 + 1
   shpPos.Height = shpPos.Width

   
   lineAlign1.X1 = 0 ' cross
   lineAlign1.Y1 = 0
   lineAlign1.X2 = imgInput.ScaleWidth
   lineAlign1.Y2 = imgInput.ScaleHeight
   
   lineAlign2.X1 = 0 ' cross
   lineAlign2.Y1 = imgInput.ScaleHeight
   lineAlign2.X2 = imgInput.ScaleWidth
   lineAlign2.Y2 = 0

   LineAlign3.X1 = 0 ' horiz
   LineAlign3.Y1 = imgInput.ScaleHeight / 2
   LineAlign3.X2 = imgInput.ScaleWidth
   LineAlign3.Y2 = imgInput.ScaleHeight / 2

   LineAlign4.X1 = imgInput.ScaleWidth / 2
   LineAlign4.Y1 = 0 ' vert
   LineAlign4.X2 = imgInput.ScaleWidth / 2
   LineAlign4.Y2 = imgInput.ScaleHeight
   

   txtLaserAngleX.Text = testdot(0).Tag
End Sub


Private Sub cmdStep_Click()
' 1) change the scanning area
   If (cur_x + step_x > imgInput.ScaleWidth - padding) Then ' overrun right
      cur_x = padding
      If (cur_y + step_y > imgInput.ScaleHeight - padding) Then ' overrun height
         cur_y = padding
      Else
         cur_y = cur_y + step_y
      End If
   Else
      cur_x = cur_x + step_x
   End If
   linePos.X2 = cur_x - padding
   linePos.Y2 = cur_y - padding
   shpPos.Left = linePos.X2 - 1
   shpPos.Top = linePos.Y2 - 1
   txtCurPosition.Text = "(" & cur_x & "," & cur_y & ")"
   
   
' 2) determine colors
   Dim color As Double, r As Integer, g As Integer, b As Integer
'  color = GetPixel(imgInput.hWnd, x, y)
   color = imgInput.Point(cur_x, cur_y)
   r = color And &HFF&
   g = (color And &HFF00&) / 2 ^ 8
   b = (color And &HFF0000) / 2 ^ 16
   
   txtCurColor.Text = PadHex(r) & PadHex(g) & PadHex(b)
   shpColor(0).FillColor = fixcolor(color)
   
   ' top row
   shpColor(1).FillColor = fixcolor(imgInput.Point(cur_x - 1, cur_y - 1))
   shpColor(2).FillColor = fixcolor(imgInput.Point(cur_x, cur_y - 1))
   shpColor(3).FillColor = fixcolor(imgInput.Point(cur_x + 1, cur_y - 1))
   
   ' middle sides (0 = center)
   shpColor(4).FillColor = fixcolor(imgInput.Point(cur_x - 1, cur_y))
   shpColor(5).FillColor = fixcolor(imgInput.Point(cur_x + 1, cur_y))
      
   ' bottom row
   shpColor(6).FillColor = fixcolor(imgInput.Point(cur_x - 1, cur_y + 1))
   shpColor(7).FillColor = fixcolor(imgInput.Point(cur_x, cur_y + 1))
   shpColor(8).FillColor = fixcolor(imgInput.Point(cur_x + 1, cur_y + 1))
   
   
   
   
   
   ' 3) determine if the laser is there, that's the intersect
   ' if lots more red than any other color, then assume the laser is there.
   If (r > g * 1.5 And r > b * 1.5) Then
      ' 4) calculate angles
      Dim cameraOffsetX As Double
      cameraOffsetX = 90 - txtFOVCamera.Text / 2 ' assuming camera center is perpendicular to leg c
      txtScanAngleX.Text = cameraOffsetX + ((cur_x / (imgInput.ScaleWidth)) * txtFOVCamera.Text)
'      txtScanAngleZ.Text = (cur_y / (imgInput.ScaleHeight)) * txtFOVCamera.Text
      
      
      Dim angle_A As Double, dist_b As Double
      If (txtLaserAngleX.Text >= 90 And txtLaserAngleX.Text < 180 And txtScanAngleX.Text >= 0 And txtScanAngleX.Text < 90) Then
         angle_A = 180 - txtLaserAngleX.Text
         dist_b = (txtDistBetween.Text * Sin(DtoR(txtScanAngleX.Text))) / Sin(DtoR(180 - angle_A - txtScanAngleX.Text))
         txtDistance.Text = dist_b * Sin(DtoR(angle_A))
      Else
         txtDistance.Text = "-1"
      End If

      MsgBox "hit at (" & cur_x & ", " & cur_y & ")"
   End If
End Sub


' administrative stuff
Private Sub cmdStart_Click()
   If (tmrMain.Enabled = True) Then
      tmrMain.Enabled = False
      cmdStart.Caption = "Start"
   Else
      tmrMain.Enabled = True
      cmdStart.Caption = "Stop"
   End If
End Sub

Private Sub tmrMain_Timer()
   cmdStep_Click
End Sub


' generic functions
Function DtoR(degrees As Double)
   DtoR = degrees * (3.141592654 / 180)
End Function

Function RtoD(radians As Double)
   RtoD = radians * (180 / 3.141592654)
End Function

Function PadHex(value As Integer, Optional length As Integer = 2) As String
   ' returns a 0 padded hex value
   PadHex = Hex(value)
   Do While (Len(PadHex) < length)
      PadHex = "0" & PadHex
   Loop
End Function

Function fixcolor(color As Double) As Double
   If (color = -1 Or color > Val("&hffffff")) Then
      fixcolor = Val("&hFF00F6") ' todo: fix this
   Else
      fixcolor = color
   End If
End Function