#!/usr/bin/env python

import rospy
import pibot_msgs.msg

class MagCalibration:
  def __init__(self):
    rospy.init_node("simple_magnetometer_calibration")
    rospy.loginfo("Rotate the mag till there is no more output.")
    self.MILIGAUSS_TO_TESLA_SCALE = 0.0000001
    self.mxh = 0
    self.myh = 0
    self.mzh = 0
    self.mxl = 0
    self.myl = 0
    self.mzl = 0
    self.raw_mag_sub = rospy.Subscriber('raw_imu', pibot_msgs.msg.RawImu, self.rawCallback)
    rospy.spin()

  def rawCallback(self, raw_imu):
    mx = raw_imu.raw_magnetic_field.x * self.MILIGAUSS_TO_TESLA_SCALE
    my = raw_imu.raw_magnetic_field.y * self.MILIGAUSS_TO_TESLA_SCALE
    mz = raw_imu.raw_magnetic_field.z * self.MILIGAUSS_TO_TESLA_SCALE

    update = False

    if (mx > self.mxh):
      self.mxh = mx
      update = True
    elif (mx < self.mxl):
      self.mxl = mx
      update = True

    if (my > self.myh):
      self.myh = my
      update = True
    elif ( my < self.myl):
      self.myl = my
      update = True

    if (mz > self.mzh):
      self.mzh = mz
      update = True
    elif(mz < self.mzl):
      self.mzl = mz
      update = True

    if (update):
      rospy.loginfo("")
      print 'X-> Min: %.9f, Max: %.9f' % (self.mxl, self.mxh)
      print 'Y-> Min: %.9f, Max: %.9f' % (self.myl, self.myh)
      print 'Z-> Min: %.9f, Max: %.9f' % (self.mzl, self.mzh)


if __name__ == '__main__':
  try:
    MagCalibration()
  except rospy.ROSInterruptException:
    pass
