import { alert, confirm } from '../js/dialogs.js'

export default {
  methods: {
    //--------------------------------------------------------
    // Level 1
    //--------------------------------------------------------
    async sendCommandUI(cmd) {
      this.consoleLogAppend('>>> ' + cmd)
      if (this.portIsOpened) {

        //if (cmd == '$H') {
        //  this.machineState.status = 'Homing'
        //}

        await this.sendCommand(cmd)
        this.consoleLogAppend(this.request)
        if (cmd[0] == 'M') {
          await this.sendCommand('$G')
        }

        if (cmd !== '?') {
          await this.sendCommand('?')
        }

        while ((this.machineState.status !== 'Idle') && ( this.machineState.status !== 'Alarm') ) {
          await new Promise(r => setTimeout(r, 100));
          await this.sendCommand('?')
        }
      }
    },

    async sendCommands(cmdArray, wailIdle=false) {
      if (this.pnpProcessing.stopFlag == true) {
        return
      }

      // reset flags
      this.gCodeProcess.stopFlag = false
      this.gCodeProcess.pauseFlag = false

      for (let cmd of cmdArray) {
        console.log(cmd)
        await this.sendCommand(cmd)
        if (cmd !== '?') {
          await this.sendCommand('?')
        }
        if (wailIdle) {
          while (this.machineState.status !== 'Idle') {
            await new Promise(r => setTimeout(r, 100));
            await this.sendCommand('?')
          }
        }
        while ((this.gCodeProcess.pauseFlag == true) || (this.pnpProcessing.pauseFlag == true)) {
          //Wait
          await new Promise(resolve => setTimeout(resolve, 200))
        }
        if ((this.gCodeProcess.stopFlag == true) || (this.pnpProcessing.stopFlag == true)) {
          break
        }
      }

      // reset flags
      this.gCodeProcess.stopFlag = false
      this.gCodeProcess.pauseFlag = false
    },

    /////
    async sendCommandsPool(commands) {
      console.log('sendCommandsPool Start')
      for (let cmd of commands) {
        await this.sendCommand(cmd)
      }
      await this.sendCommands(['?'], true)
      console.log('sendCommandsPool Stop')
    },

    async sendCommand(cmd) {
      if (this.portIsOpened) {
        if (cmd == '$H') {
          this.machineState.status = 'Homing'
        }
        
        let command = Uint8Array.from(Array.from(cmd + "\n").map(letter => letter.charCodeAt(0)));
        await this.portConnection.write(command)
        await this.readRequest()
      }
    },

    async readRequest() {
      let request = await this.portConnection.read()
      this.parseRequest(request)
    },

    parseRequest(request) {
      let decoder = new TextDecoder();
      let str = decoder.decode(request);
      this.request = str

      // Parse state get after command '?'
      // <Idle|MPos:30.000,10.000,0.000,0.000>
      let regexp = /<([a-z,A-Z]*)\|MPos:(.*)>/g
      let matchAll = str.matchAll(regexp)
      matchAll = Array.from(matchAll)
      if (matchAll.length == 1) {
        if (matchAll[0].length == 3) {
          this.machineState.status = matchAll[0][1]
          let pos = matchAll[0][2].split(',')
          this.machineState.pos.x = parseFloat(pos[0])
          this.machineState.pos.y = parseFloat(pos[1])
          this.machineState.pos.z = parseFloat(pos[2])
          this.machineState.pos.a = parseFloat(pos[3])
        }
      } else {
        // Parse information get after commands '$I' or '$G'
        let regexp = /\[([a-z,A-Z, ]*):(.*)]/g
        let matchAll = str.matchAll(regexp)
        matchAll = Array.from(matchAll)
        if (matchAll.length > 0) {
          for (let item of matchAll) {
            if (item.length == 3) {
              this.machineState.info[item[1]] = item[2]
            }
          }
        } else {
          // Parse settings get after command '$$' or '$[0..9]*' ($10, $23, ...)
          let regexp = /\$([0-9]*)=(.*)/g
          let matchAll = str.matchAll(regexp)
          matchAll = Array.from(matchAll)
          if (matchAll.length > 0) {
            for (let item of matchAll) {
              if (item.length == 3) {
                this.machineState.settings[item[1]] = parseFloat(item[2])
              }
            }            
          }
        }
      }
    },

    //--------------------------------------------------------
    // Level 2
    //--------------------------------------------------------
    fetch_execute_async: async function (url, options) {
      this.loading = true
      try {
        let response = await fetch(url, options)
        let result = await response.json()
        return result
      }
      catch (error) {
        console.log(error)
      }
      finally {
        this.loading = false
      }
    },

    async getCVData(number) {
      let url = 'http://localhost:5000/cv/'+number+'?'+Date.now()
        let postdata = this.cvParams[number]
      return await this.fetch_execute_async( url, { method: 'POST', credentials: 'same-origin', cache: 'no-cache', headers: {'Content-Type': 'application/json;charset=utf-8'}, body: JSON.stringify(postdata)} )
    },

    pnpGoToZero_GCODE() {
      let commands = []
      let safePosZ = parseFloat(this.settings.heads[0].safePosZ)
      commands.push(`G0 Z${safePosZ}`)
      commands.push(`G0 X0 Y0`)
      commands.push(`G0 Z0`)
      return commands
    },

    async pnpGoToZero() {
      let commands = this.pnpGoToZero_GCODE()
      await this.sendCommands(commands, true)
    },

    pnpGoToParking_GCODE() {
      let commands = []
      commands.push(`G0 Z${this.settings.parking.pos.z}`)
      commands.push(`G0 X${this.settings.parking.pos.x} Y${this.settings.parking.pos.y} A0`)
      return commands
    },

    async pnpGoToParking() {
      let commands = this.pnpGoToParking_GCODE()
      await this.sendCommands(commands, true)
    },

    pnpPumpOn_GCODE() {
      let commands = []
      commands.push(this.settings.pump.On)
      commands.push(`$G`)
      return commands
    },

    async pnpPumpOn() {
      let commands = this.pnpPumpOn_GCODE()
      await this.sendCommands(commands, true)
    },

    pnpPumpOff_GCODE() {
      let commands = []
      commands.push(this.settings.pump.Off)
      commands.push(`$G`)
      return commands
    },

    async pnpPumpOff() {
      let commands = this.pnpPumpOff_GCODE()
      await this.sendCommands(commands, true)
    },

    pnpValveOn_GCODE(head) {
      let commands = []
      commands.push(`G04 P${head.pauseBeforeOn}`)
      commands.push(head.valveOn)
      commands.push(`G04 P${head.pauseAfterOn}`)
      commands.push(`$G`)
      return commands
    },
    
    async pnpValveOn(head) {
     let commands = this.pnpValveOn_GCODE(head)
     await this.sendCommands(commands, true)
    },

    pnpValveOff_GCODE(head) {
      let commands = []
      commands.push(`G04 P${head.pauseBeforeOff}`)
      commands.push(head.valveOff)
      commands.push(`G04 P${head.pauseAfterOff}`)
      commands.push(`$G`)
      return commands
    },

    async pnpValveOff(head) {
      let commands = this.pnpValveOff_GCODE(head)
      await this.sendCommands(commands, true)
    },

    pnpLightOn_GCODE(camN) {
      let commands = []
      commands.push(this.settings.cams[camN].lightOn)
      commands.push(`$G`)
      return commands
    },
    
    async pnpLightOn(camN) {
     let commands = this.pnpLightOn_GCODE(camN)
     await this.sendCommands(commands, true)
    },

    pnpLightOff_GCODE(camN) {
      let commands = []
      commands.push(this.settings.cams[camN].lightOff)
      commands.push(`$G`)
      return commands
    },

    async pnpLightOff(camN) {
      let commands = this.pnpLightOff_GCODE(camN)
      await this.sendCommands(commands, true)
    },


    pnpSafetyUp_GCODE(a) {
      let safePosZ = parseFloat(this.settings.heads[0].safePosZ)
      let cmd = `G0 Z${safePosZ}`
      if (!isNaN(a)) {
        cmd = cmd + ` A${a}`
      }
      let commands = [cmd]
      return commands
    },

    // this.pnpSafetyUp()    - just Up to Safety Z
    // this.pnpSafetyUp(0)   - Up to Safety Z and Rotate to A=0
    async pnpSafetyUp(a) {
      let commands = this.pnpSafetyUp_GCODE(a)
      await this.sendCommands(commands, true)
    },


    /////
    pnpSafetyGoToXYZ_GCODE(x, y, z) {
      let commands = []
      //let safePosZ = parseFloat(this.settings.heads[0].safePosZ)
      //commands.push(`G0 Z${safePosZ}`)
      //commands.push(`G0 X${x} Y${y}`)


      //commands.push(...this.pnpGoToXYZAF_GCODE(NaN, NaN, safePosZ, NaN, 5500))
      commands.push(...this.pnpSafetyUp_GCODE())
      commands.push(...this.pnpGoToXYZAF_GCODE(x, y, NaN, NaN, 5500))

      if (!isNaN(z)) {
        //commands.push(`G0 Z${z}`)
        commands.push(...this.pnpGoToXYZAF_GCODE(NaN, NaN, z, NaN, 5500))
      }
      return commands
    },

    // this.pnpSafetyGoToXYZ(x=10, y=10, z=10)
    // this.pnpSafetyGoToXYZ(10, 0, 10)
    // this.pnpSafetyGoToXYZ(x=10, y=10, z=NaN)  - finally Z = safePosZ
    // this.pnpSafetyGoToXYZ(10, 0, z=NaN)       - finally Z = safePosZ
    async pnpSafetyGoToXYZ(x, y, z) {
      let commands = this.pnpSafetyGoToXYZ_GCODE(x, y, z)
      await this.sendCommands(commands, true)
    },

    /////
    pnpGoToXYZA_GCODE(x, y, z, a) {
      let cmd = 'G0'
      if (!isNaN(x)) cmd = cmd + ` X${x}`
      if (!isNaN(y)) cmd = cmd + ` Y${y}`
      if (!isNaN(z)) cmd = cmd + ` Z${z}`
      if (!isNaN(a)) cmd = cmd + ` A${a}`
      let commands = [cmd]
      return commands
    },

    // this.pnpGoToXYZA(x=NaN, y=NaN, z=-25, a=0)
    // this.pnpGoToXYZA(10, 20, NaN, NaN)
    // this.pnpGoToXYZA(NaN, NaN, 20, 0)
    async pnpGoToXYZA(x, y, z, a) {
      let commands = this.pnpGoToXYZA_GCODE(x, y, z, a)
      await this.sendCommands(commands, true)
    },

/*
    pnpGoToXYZF_GCODE(x, y, z, feed) {
      let cmd = 'G1'
      if (!isNaN(x)) cmd = cmd + ` X${x}`
      if (!isNaN(y)) cmd = cmd + ` Y${y}`
      if (!isNaN(z)) cmd = cmd + ` Z${z}`
      if (!isNaN(feed)) cmd = cmd + ` F${feed}`
      let commands = [cmd]
      return commands
    },

    async pnpGoToXYZF(x, y, z, feed) {
      let commands = this.pnpGoToXYZF_GCODE(x, y, z, feed)
      await this.sendCommands(commands, true)
    },
*/

    pnpGoToXYZAF_GCODE(x, y, z, a, feed=500) {
      let cmd = 'G1'
      if (!isNaN(x)) cmd = cmd + ` X${x}`
      if (!isNaN(y)) cmd = cmd + ` Y${y}`
      if (!isNaN(z)) cmd = cmd + ` Z${z}`
      if (!isNaN(a)) cmd = cmd + ` A${a}`
      if (!isNaN(feed)) cmd = cmd + ` F${feed}`
      let commands = [cmd]
      return commands
    },


    //--------------------------------------------------------
    // Level 3
    //--------------------------------------------------------

    //@pnpDo
    async pnpGoToBottomCam() {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let bottomCamID = 1
        let x, y, z
        x = parseFloat(this.settings.cams[bottomCamID].pos.x)
        y = parseFloat(this.settings.cams[bottomCamID].pos.y)
        z = parseFloat(this.settings.cams[bottomCamID].pos.z)

        // Move to Bottom Cam X,Y
        await this.pnpSafetyGoToXYZ(x, y, NaN, NaN)

        // Light On
        await this.pnpLightOn(bottomCamID)

        // Move to Bottom Cam Z
        await this.pnpGoToXYZA(NaN, NaN, z, NaN)
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    // ---------------------------------
    // Go to feeder on safety Z position
    // ---------------------------------
    getFeederPos(feeder) {
        let x = 0
        let y = 0

        // Type Liner
        if (feeder.type == 0) {
          x = feeder.pos.x
          y = feeder.pos.y
        }

        // Type Push-Pull
        if (feeder.type == 1) {
          for (let point of feeder.pathPoints) {
            if (point.pick == true) {
              x = point.x
              y = point.y
            }
          }
        }

        // Type Heap
        if (feeder.type == 2) {
          x = feeder.pos.x
          y = feeder.pos.y
        }

        return {x:x, y:y}
    },

    async pnpGoToFeeder(feeder) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true
        let pos = this.getFeederPos(feeder)
        await this.pnpSafetyGoToXYZ(pos.x, pos.y, NaN)
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    async pnpGoToFeederCam(feeder) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let pos = this.getFeederPos(feeder)

        pos.x = parseFloat(pos.x) - parseFloat(this.settings.cams[0].offset.x)
        pos.y = parseFloat(pos.y) - parseFloat(this.settings.cams[0].offset.y)

        await this.pnpSafetyGoToXYZ(pos.x, pos.y, NaN)

      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    // ---------------------------------
    // Pick element rom feeder
    // ---------------------------------
    async pnpPickFromFeeder(feeder, headN=0) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let x, y, z
        let head = this.settings.heads[headN]

        // Type Liner
        if (feeder.type == 0) {
          if (parseInt(feeder.current)+1 < parseInt(feeder.capacity)) {

            if (this.valvesState[headN] == false) {
              x = parseFloat(feeder.pos.x) + parseFloat(feeder.step.x) * parseFloat(feeder.current)
              y = parseFloat(feeder.pos.y) + parseFloat(feeder.step.y) * parseFloat(feeder.current)
              z = parseFloat(feeder.pos.z) + parseFloat(feeder.step.z) * parseFloat(feeder.current)

              await this.pnpSafetyUp(0)
              await this.pnpSafetyGoToXYZ(x, y, z)
              await this.pnpValveOn(head)
              await this.pnpSafetyUp()

              feeder.current = parseInt(feeder.current)+1
            } else {
              await alert('Valve is On!', 'Warning', 'warning')
              return false
            }
          } else {
            await alert('Feeder is empty', 'Warning', 'warning')
            return false
          }
        }

        // Type Push-Pull
        if (feeder.type == 1) {
          await this.pnpSafetyUp(0)

          let commands = []
          for (let point of feeder.pathPoints) {
            //await this.pnpGoToXYZA(point.x, point.y, NaN, NaN)
            //await this.pnpGoToXYZA(NaN, NaN, point.z, NaN)
            
            commands.push(...this.pnpGoToXYZAF_GCODE(point.x, point.y, NaN, NaN, 5500))
            commands.push(...this.pnpGoToXYZAF_GCODE(NaN, NaN, point.z, NaN, 5500))

            if (point.pick == true) {
              //await this.pnpValveOn(head)
              commands.push(...this.pnpValveOn_GCODE(head))
            }
          }
          console.log(commands)
          await this.sendCommandsPool(commands)

          await this.pnpSafetyUp()
        }

        // Type Heap
        if (feeder.type == 2) {
          let topCamID = 0

          await this.pnpSafetyUp(0)
          await this.pnpLightOn(topCamID)
          await this.pnpGoToFeederCam(feeder)
          
          // Pause before make Photo :)
          await new Promise(r => setTimeout(r, this.settings.cv[topCamID].pauseBefore*1000))
          // Get CV data
          let cvData = await this.getCVData(topCamID)
          
          await this.pnpLightOff(topCamID)

          let rectObject
          let rectObjectDistance = 10000

          // If object is found
          if (typeof cvData.objects !== 'undefined') {
            // If rect(s) is found
            if (typeof cvData.objects.rect !== 'undefined') {
              let wx = parseFloat(feeder.pos.wx) / 2.0
              let wy = parseFloat(feeder.pos.wy) / 2.0

              for (let obj of cvData.objects.rect) {
                let dx = Math.round(obj.dx*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0
                let dy = Math.round(obj.dy*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0

                let distance = Math.sqrt(dx*dx + dy*dy)

                if ((Math.abs(dx) < wx) && (Math.abs(dy) < wy)) {
                  if (distance < rectObjectDistance) {
                    rectObject = Object.assign({}, obj)
                    rectObject.dxmm = dx
                    rectObject.dymm = dy
                    rectObjectDistance = distance
                  }
                }
              }

            }
          }

          if (rectObject) {
            //console.log(rectObject)
            let x = parseFloat(feeder.pos.x) - rectObject.dxmm
            let y = parseFloat(feeder.pos.y) + rectObject.dymm
            let z = parseFloat(feeder.pos.z) + parseFloat(feeder.elementHeight)
            let a = -rectObject.angle / 360
            
            //console.log(x,y,x,a)
            await this.pnpSafetyGoToXYZ(x, y, z)
            await this.pnpValveOn(head)
            await this.pnpSafetyUp()
            await this.pnpGoToXYZA(NaN, NaN, NaN, a)

          } else {
            await alert('Can not see element in feeder "' + feeder.name + '"!', 'Warning', 'warning')
          }

        }

        return true
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    // ---------------------------------
    // Correction of Feeder Position using Computer Vision
    // ---------------------------------
    async pnpAdjFeederPositionCV(feeder) {
      let topCamID = 0

      // Light On
      await this.pnpLightOn(topCamID)
      await this.pnpGoToFeederCam(feeder)
      
      // Pause before make Photo :)
      await new Promise(r => setTimeout(r, this.settings.cv[topCamID].pauseBefore*1000))
      // Get CV data
      let cvData = await this.getCVData(topCamID)
      let rectObject
      let rectObjectDistance = 10000

      // If object is found
      if (typeof cvData.objects !== 'undefined') {
        // If rect(s) is found
        if (typeof cvData.objects.rect !== 'undefined') {
          for (let obj of cvData.objects.rect) {
            let dx = Math.round(obj.dx*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0
            let dy = Math.round(obj.dy*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0

            let distance = Math.sqrt(dx*dx + dy*dy)

            if (distance < 2.0) {
              if (distance < rectObjectDistance) {
                rectObject = Object.assign({}, obj)
                rectObject.dxmm = dx
                rectObject.dymm = dy
                rectObjectDistance = distance
              }
            }
          }
        }        
      }

      if (rectObject) {
        // Line Feeder
        if (feeder.type == 0) {
          feeder.pos.x = (parseFloat(feeder.pos.x)- rectObject.dxmm).toFixed(3)
          feeder.pos.y = (parseFloat(feeder.pos.y) + rectObject.dymm).toFixed(3)
        }
        // Push-Pull Feeder
        if (feeder.type == 1) {
          let pointY = 0
          for (let point of feeder.pathPoints) {
            point.x = (parseFloat(point.x) - rectObject.dxmm).toFixed(3)
            if (point.pick == true) {
              pointY = point.y
            }
          }
          for (let point of feeder.pathPoints) {
            if (point.y == pointY) {
              point.y = (parseFloat(point.y) + rectObject.dymm).toFixed(3)
            }
          }
        }

        await this.pnpGoToFeederCam(feeder)
      } else {
        await alert('Can not see element in feeder "' + feeder.name + '"!', 'Warning', 'warning')
      }
      
      await this.pnpLightOff(topCamID)
    },

    async pnpAdjFeedersPositionCV(feeders) {
      for (let feeder of feeders) {
        if (feeder.active == true) {
          await this.pnpAdjFeederPositionCV(feeder)
        }
      }
    },


    async pnpAdjPCBPositionCV(project) {
      let topCamID = 0

      // Find fiducials in elements array
      let fiducials = []
      for (let element of project.elements) {
        if (element.caption.toLowerCase().includes('fiducial')) {
            fiducials.push(element)
        }
      }

      // If fiducials is found
      if (fiducials.length > 0) {
        let xOffset = 0
        let yOffset = 0
        let fiducialCount = 0

        for (let fiducial of fiducials) {
          //
          await this.pnpLightOn(topCamID)
          await this.pnpGoToElementCam(fiducial)

          // Pause before make Photo :)
          await new Promise(r => setTimeout(r, 2*this.settings.cv[topCamID].pauseBefore*1000))
          // Get CV data
          let cvData = await this.getCVData(topCamID)
          await this.pnpLightOff(topCamID)

          //console.log(cvData)

          let circleObject
          let circleObjectDistance = 10000

          // If object is found
          if (typeof cvData.objects !== 'undefined') {
            // If circle(s) is found
            if (typeof cvData.objects.circle !== 'undefined') {
              for (let obj of cvData.objects.circle) {

                let dx = Math.round(obj.dx*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0
                let dy = Math.round(obj.dy*1000.0 / this.settings.cams[topCamID].ppmm) / 1000.0

                let distance = Math.sqrt(dx*dx + dy*dy)

                if (distance < 2.0) {
                  if (distance < circleObjectDistance) {
                    circleObject = Object.assign({}, obj)
                    circleObject.dxmm = dx
                    circleObject.dymm = dy
                    circleObjectDistance = distance
                  }
                }

              }
            }        
          }

          if (circleObject) {
            //console.log(circleObject.dxmm, circleObject.dymm)
            xOffset += circleObject.dxmm
            yOffset += circleObject.dymm
            fiducialCount += 1
          } else {
            await alert('Can not see Fiducials on the PCB!', 'Warning', 'warning')
          }

        }

        // Set PSB Offset
        if (fiducialCount > 0) {
          project.pcb.pos.x = (xOffset / fiducialCount).toFixed(3)
          project.pcb.pos.y = (yOffset / fiducialCount).toFixed(3)
        }

      } else {
        await alert('Fiducials is not found! One or more elements must contain \'FIDUCIAL\' in field \'Caption\'!', 'Warning', 'warning')
      }
    },

    // ---------------------------------
    // Adjust position of picked element 
    // ---------------------------------
    async pnpAdjPosition() {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let bottomCamID = 1
        let x, y, z, a
        let cvData

        x = parseFloat(this.settings.cams[bottomCamID].pos.x)
        y = parseFloat(this.settings.cams[bottomCamID].pos.y)
        z = parseFloat(this.settings.cams[bottomCamID].pos.z)
        a = this.machinePos.a
        //let aDegrees = a * 360.0

        await this.pnpGoToBottomCam()

        // Adjust position and rotation in 3 iterations
        for (let i=0; i<3; i++) {
          // Pause before make Photo :)
          await new Promise(r => setTimeout(r, this.settings.cv[bottomCamID].pauseBefore*1000))
          // Get CV data
          cvData = await this.getCVData(bottomCamID)
          console.log(cvData)
          // If object is found
          if (typeof cvData.objects.object !== 'undefined') {
            // Adjust Position X,Y
            let dx = Math.round(cvData.objects.object.dx*1000.0 / this.settings.cams[bottomCamID].ppmm) / 1000.0
            let dy = Math.round(cvData.objects.object.dy*1000.0 / this.settings.cams[bottomCamID].ppmm) / 1000.0
            x = x - dx
            y = y - dy

            // Adjust Rotation
            let da = cvData.objects.object.angle_degrees

            // Align to vertical or horizontal
            if (da < -45.0) {
              da = 90.0 + da
            }
            if (da > 45.0) {
              da = -90.0 + da
            }
            // Recalculate to turn
            da = da / 360
            // Calculate absolute a
            a = a - da

            if ( (Math.abs(dx) < 5.0) && (Math.abs(dy) < 5.0) && (Math.abs(da) < 0.25) )  { // if dx, dy, da is in adequate range
              // Move and rotate
              await this.pnpGoToXYZA(x, y, NaN, a)
            }
          }
        }
        //

        // Light off
        await this.pnpLightOff(bottomCamID)

        let el_x = parseFloat(this.machinePos.x) - parseFloat(this.settings.cams[bottomCamID].pos.x)
        let el_y = parseFloat(this.machinePos.y) - parseFloat(this.settings.cams[bottomCamID].pos.y)
        let element_offsets = {x: el_x, y: el_y}
        return element_offsets
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }


    },

    // ---------------------------------
    // To Basket
    // ---------------------------------
    async pnpDropIntoBasket() {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let x = this.settings.basket.pos.x
        let y = this.settings.basket.pos.y
        let z = this.settings.basket.pos.z

        await this.pnpSafetyGoToXYZ(x, y, z)

        for (let head of this.settings.heads) {
          await this.pnpValveOff(head)
        }
        await this.pnpSafetyUp(0)
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    //--------------------------------------------------------
    async pnpGoToElement(element) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let offsetPCB = this.project.pcb.pos
        let x = parseFloat(element.pos.x) + parseFloat(offsetPCB.x)
        let y = parseFloat(element.pos.y) + parseFloat(offsetPCB.y)

        await this.pnpSafetyGoToXYZ(x, y, NaN)
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    async pnpGoToElementCam(element) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let offsetPCB = this.project.pcb.pos
        let x = parseFloat(element.pos.x) + parseFloat(offsetPCB.x) - parseFloat(this.settings.cams[0].offset.x)
        let y = parseFloat(element.pos.y) + parseFloat(offsetPCB.y) - parseFloat(this.settings.cams[0].offset.y)

        await this.pnpSafetyGoToXYZ(x, y, NaN)
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },
    //--------------------------------------------------------

    getNozzle(id) {
      for (let nozzle of this.settings.nozzles) {
        if (nozzle.id == id) {
          return nozzle
        }
      }
    },

    getNozzleOffset(nozzle, a) {
      if (a < 0) {
        a = 1.0 + a
      }
      for (let row of nozzle.calibrationData) {
        if (row.a >= a) {
          return row
        }
      }
    },


    async pnpPlaceElement(element) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let head = this.settings.heads[0]
        let PCBTableZ = parseFloat(this.settings.table.pos.z)

        let feederId = element.feeder
        let offsetPos = {x:0, y:0}
        let a = parseFloat(element.pos.a)

        for (let feeder of this.project.feeders) {
          if (feeder.id == feederId) {

            if (await this.pnpPickFromFeeder(feeder) == false) {
              return false
            }
            
            // Rotate element if need
            if (a !== 0.0) {
              await this.pnpGoToXYZA(NaN, NaN, NaN, a) 
            }

            // Correct Position
            if (feeder.posCorrection == true) {
              offsetPos = await this.pnpAdjPosition() // Adjust position
            } else {
              // Get nuzzle offsets here
              let nozzle = this.getNozzle(feeder.nozzle)
              let nozzleOffset = this.getNozzleOffset(nozzle, a)
              offsetPos.x = nozzleOffset.dx
              offsetPos.y = nozzleOffset.dy
            }

            let offsetPCB = this.project.pcb.pos
    
            let x = parseFloat(element.pos.x) + parseFloat(offsetPos.x) + parseFloat(offsetPCB.x)
            let y = parseFloat(element.pos.y) + parseFloat(offsetPos.y) + parseFloat(offsetPCB.y)
            let z = PCBTableZ + parseFloat(offsetPCB.z) + parseFloat(feeder.elementHeight)

            await this.pnpSafetyGoToXYZ(x, y, z)
            await this.pnpValveOff(head)
            await this.pnpSafetyUp()
            await this.pnpGoToXYZA(NaN, NaN, NaN, 0) 

          }
        }
        return true
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },
    //--------------------------------------------------------

    async pnpAutocalibrateNozzle(nozzle) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {
        this.pnpProcessing.runFlag = true

        let bottomCamID = 1
        let ppmm = this.settings.cams[bottomCamID].ppmm

        await this.pnpSafetyUp(0)
        await this.pnpGoToBottomCam()

        for (let row of nozzle.calibrationData) {
          //if (row.a != 0) {
            await this.pnpGoToXYZA(NaN, NaN, NaN, row.a)
            // Pause before make Photo :)
            await new Promise(r => setTimeout(r, this.settings.cv[bottomCamID].pauseBefore*1000))
            let cvData = await this.getCVData(bottomCamID)
            if (typeof cvData.objects.nozzle != 'undefined') {
              row.dx = -(cvData.objects.nozzle.dx / ppmm).toFixed(3)
              row.dy = -(cvData.objects.nozzle.dy / ppmm).toFixed(3)
            } else {
              await alert("Autocalibration is failed! Nozzle not found!", 'Error', 'error')
              return
            }

          //}
        }
        
        // Light off
        await this.pnpLightOff(bottomCamID)
        await this.pnpSafetyUp(0)

        await alert("Calibration has been completed", 'Completed', 'success')
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    },

    getNozzlePlace(nozzleId) {
      for (let place of this.settings.nozzlesHolder.places) {
        if (place.nozzleId == nozzleId) {
          return place
        }
      }
      return null
    },

    pnpGetEmptyNozzlePlace() {
      let emptyPlace = null

      // Return true if head without Nozzle
      if (this.settings.heads[0].nozzle == -1) {
        return true
      }

      // Find empty place
      for (let place of this.settings.nozzlesHolder.places) {
        if (place.nozzleId == -1) {
          emptyPlace = place
          break
        }
      }

      if (emptyPlace == null) {
        alert('Not found the empty place!', 'Error', 'error')
        return null
      }
      return emptyPlace
    },

    async pnpPullOffNozzle(nozzlePlace) {
      let emptyPlace = this.pnpGetEmptyNozzlePlace()
      if (emptyPlace === true) { // Head without Nozzle
        return true
      }

      if (emptyPlace == null) { // Not found the empty place!
        return false
      }

      // Pull off the Nozzle
      let commands = []
      for (let i = emptyPlace.points.length-1; i >= 0; i--) {
        let point = emptyPlace.points[i]
        if (i == emptyPlace.points.length-1) {
          //commands.push(...this.pnpGoToXYZAF_GCODE(point.x, point.y, point.z, NaN, 5500))
          commands.push(...this.pnpSafetyGoToXYZ_GCODE(point.x, point.y, point.z))
          
          //await this.pnpSafetyGoToXYZ(point.x, point.y, point.z)
        } else {
          //await this.pnpGoToXYZF(point.x, point.y, point.z, nozzlePlace.feed)
          commands.push(...this.pnpGoToXYZAF_GCODE(point.x, point.y, point.z, NaN, nozzlePlace.feed))
        }
      }
      let safePosZ = parseFloat(this.settings.heads[0].safePosZ)
      commands.push(...this.pnpGoToXYZAF_GCODE(NaN, NaN, safePosZ, NaN, 5500))

      await this.sendCommandsPool(commands)

      // Set current Nozzle to empty place
      emptyPlace.nozzleId = this.settings.heads[0].nozzle
      // Set head empty
      this.settings.heads[0].nozzle = -1

      return true
    },

    async pnpPutOnNozzle(nozzlePlace) {
      if (nozzlePlace.nozzleId == -1) {
        alert('Specified Nozzle place is empty!', 'Error', 'error')
        return
      }
      if (nozzlePlace.points.length == 0) {
        alert('Not specified the path to take!', 'Error', 'error')
        return
      }
      
      if (await this.pnpPullOffNozzle(nozzlePlace) == false) {
          return
      }

      // Put on the Nozzle
      console.log(nozzlePlace)
      let commands = []
      for (let i = 0; i < nozzlePlace.points.length; i++) {
        let point = nozzlePlace.points[i]
        console.log(point)
        if (i == 0) {
          //await this.pnpSafetyGoToXYZ(point.x, point.y, point.z)
          commands.push(...this.pnpSafetyGoToXYZ_GCODE(point.x, point.y, point.z))
        } else {
          //await this.pnpGoToXYZF(point.x, point.y, point.z, nozzlePlace.feed)
          commands.push(...this.pnpGoToXYZAF_GCODE(point.x, point.y, point.z, NaN, nozzlePlace.feed))
        }
      }
      await this.sendCommandsPool(commands)

      // Set The current Nozzle
      this.settings.heads[0].nozzle = nozzlePlace.nozzleId

      // Set The empty place
      nozzlePlace.nozzleId = -1

      await this.pnpSafetyUp(0)
    },

    //--------------------------------------------------------
    async pnpJobStart(startElement = null) {
      // Prepare elements list      
      let elements = []

      if (startElement !== null) {
        elements = this.project.elements.slice(this.project.elements.indexOf(startElement), this.project.elements.length)
      } else {
        elements = this.project.elements
      }

      // Check required Nozzles in holder
      for (let element of elements) {
        if (element.active == true) {
          if (element.nozzleId != this.settings.heads[0].nozzle) {
            let nozzlePlace = this.getNozzlePlace(element.nozzleId)
            if (nozzlePlace == null) {
              let nozzle = this.getNozzle(element.nozzleId)
              alert (`Can't start Job because the nozzles holder doesn't have the required nozzle '${nozzle.name}'`, 'Warning', 'warning')
              return
            }
          }
        }
      }

      // Checking current Nozzle and empty place
      let nozzle = this.getNozzle(this.settings.heads[0].nozzle)
      let emptyPlace = this.getNozzlePlace(-1)
      await confirm(`Head is use the Nozzle '${nozzle.name}'. Empty place in the Nozzles Holder is '${emptyPlace.name}'. Is it correct? Shall we begin?`, 'Please check').then(async (res)=> {
          if (res == true) {
            // Start Job procesing
            this.pnpJobProcessing(elements)
          }
      })
    },
    //--------------------------------------------------------

    async pnpJobProcessing(elements) {
      let pnpProcessing = this.pnpProcessing.runFlag
      try {

        this.pnpProcessing.runFlag = true
        this.pnpProcessingRun()

        await this.pnpPumpOn()

        // Remember Current Nozzle
        let RememberNozzleId = this.settings.heads[0].nozzle

        for (let element of elements) {
          if (this.pnpProcessing.stopFlag == false) {

            if (element.active == true) {
              this.pnpProcessing.element = element
              this.pnpProcessing.message = `Element is being placed: ${element.name}`

              // Check Nozzle
              if (element.nozzleId != this.settings.heads[0].nozzle) {
                let nozzlePlace = this.getNozzlePlace(element.nozzleId)
                console.log('Change Nozzle')
                console.log(nozzlePlace)
                // Change Nozzle
                await this.pnpPutOnNozzle(nozzlePlace)
              }

              if (await this.pnpPlaceElement(element) == false) {
                await alert("Job is braked!", 'Warning', 'warning')
                return false
              }
            }

          }
        }

        // Restore Nozle
        if (this.settings.heads[0].nozzle != RememberNozzleId) {
          let nozzlePlace = this.getNozzlePlace(RememberNozzleId)
          await this.pnpPutOnNozzle(nozzlePlace)
        }

        await this.pnpPumpOff()
        await this.pnpGoToParking()

        if (this.pnpProcessing.stopFlag == true) {
          this.pnpProcessing.message = `The Job has been canceled on Element: ${this.pnpProcessing.element.name}`

          await alert("The Job has been canceled!", 'Canceled', 'warning')
          this.pnpProcessingReset()
        } else {
          this.pnpProcessing.message = 'Completed'
          this.pnpProcessing.element = null
          await alert("The project \"" +  this.project.name + "\" has been completed", 'Completed', 'success')
          this.pnpProcessing.message = ''
        }

        //return true
      } finally {
        this.pnpProcessing.runFlag = pnpProcessing
      }
    }

  }
}
