package main

import (
  "log"
  "fmt"
  "github.com/jacobsa/go-serial/serial"
  "dynamixel/network"
  "dynamixel/servo"
  "dynamixel/servo/xl430"
)

func GoalAndTrack(s *servo.Servo, pos int) error{
  curPos, err := s.PresentPosition()
  if err != nil {
    return err
  }
  if curPos == pos{
    fmt.Println("Already at ", pos, " nothing to do")
  }else{
    err = s.SetGoalPosition(pos)
    if err != nil {
      return err
    }
    for(curPos != pos){
      curPos, err = s.PresentPosition()
      if err != nil {
        return err
      }
      fmt.Println("Goal: ", pos, " currently at: ", curPos)
    }
  }
  return nil
}

func main() {
  options := serial.OpenOptions{
    PortName: "/dev/ttyUSB1",
    BaudRate: 1000000,
    DataBits: 8,
    StopBits: 1,
    MinimumReadSize: 0,
    InterCharacterTimeout: 100,
  }

  serial, err := serial.Open(options)
  if err != nil {
    log.Fatalf("error opening serial port: %v\n", err)
  }

  network := network.New(serial)
  servo, err := xl430.New(network, 2)
  if err != nil {
    log.Fatalf("error initializing servo: %v\n", err)
  }

  err = servo.Ping()
  if err != nil {
    log.Fatalf("error pinging servo: %v\n", err)
  }
  var ver int
  ver, err = servo.ModelNumber()
  if err != nil {
    log.Fatalf("error getting model num: %v\n", err)
  }
  fmt.Println(ver)

  err = servo.SetTorqueEnable(true)
  if err != nil {
    log.Fatalf("error setting Torque on\n", err)
  }
  err = GoalAndTrack(servo, 950)
  if err != nil {
    log.Fatalf("error setting goal position: %v\n", err)
  }

  err = GoalAndTrack(servo, 850)
  if err != nil {
    log.Fatalf("error setting goal position: %v\n", err)
  }
  err = servo.SetTorqueEnable(false)
}