#!/usr/bin/python
# -*- coding: utf-8 -*-
import sys
import getopt

sys.path.append('.')
import RTIMU
import os.path
import time
import math
import operator
import socket

sampleRate = 60.0

from functools  import reduce

IMU_IP = '127.0.0.2'
IMU_PORT = 5005

SETTINGS_FILE = 'RTIMULib'

s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)

# timers

t_print = time.time()
t_damp = time.time()
t_fail = time.time()
t_fail_timer = 0.0
t_shutdown = 0

if not imu.IMUInit():
    hack = time.time()
    imu_sentence = '$IIXDR,IMU_FAILED_TO_INITIALIZE*7C'
    if hack - t_print > 1.0:
        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))
        t_print = hack
        t_shutdown += 1
        if t_shutdown > 9:
            sys.exit(1)

imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)

poll_interval = imu.IMUGetPollInterval()

# data variables

roll = 0.0
pitch = 0.0
yaw = 0.0
heading = 0.0
rollrate = 0.0
pitchrate = 0.0
yawrate = 0.0
magnetic_deviation = -13.7

# dampening variables

t_one = 0
t_three = 0
roll_total = 0.0
roll_run = [0] * 10
heading_cos_total = 0.0
heading_sin_total = 0.0
heading_cos_run = [0] * 30
heading_sin_run = [0] * 30

# sentences produces by the imu

iihdt0 = '$IIHDT,,T*0C'
iixdr0 = '$IIXDR,A,,D,ROLL,A,,D,PTCH,A,,D,RLLR,A,,D,PTCR,A,,D,YAWR*51'
iihdt = iihdt0
iixdr = iixdr0
freq = 1

while True:
    hack = time.time()

	# if it's been longer than 5 seconds since last print

    if hack - t_damp > 5.0:

        if hack - t_fail > 1.0:
            t_one = 0
            t_three = 0
            roll_total = 0.0
            roll_run = [0] * 10
            heading_cos_total = 0.0
            heading_sin_total = 0.0
            heading_cos_run = [0] * 30
            heading_sin_run = [0] * 30
            t_fail_timer += 1
            imu_sentence = 'IIXDR,IMU_FAIL,' + str(round(t_fail_timer
                    / 60, 1))
            cs = format(reduce(operator.xor, map(ord, imu_sentence),
                        0), 'X')
            if len(cs) == 1:
                cs = '0' + cs
            imu_sentence = '$' + imu_sentence + '*' + cs
            print(imu_sentence)
            t_fail = hack
            t_shutdown += 1

    if imu.IMURead():
        data = imu.getIMUData()
        fusionPose = data['fusionPose']
        Gyro = data['gyro']
        t_fail_timer = 0.0

        if hack - t_damp > (1/sampleRate):
            roll = round(math.degrees(fusionPose[0]), 1)
            pitch = round(math.degrees(fusionPose[1]), 1)
            yaw = round(math.degrees(fusionPose[2]), 1)
            
            if yaw < 90.1:
                heading = yaw + 270 - magnetic_deviation
            else:
                heading = yaw - 90 - magnetic_deviation
            if heading > 360.0:
                heading = heading - 360.0

			# Dampening functions
            roll_total = roll_total - roll_run[t_one]
            roll_run[t_one] = roll
            roll_total = roll_total + roll_run[t_one]
            roll = roll_total / 10
            heading_cos_total = heading_cos_total \
                - heading_cos_run[t_three]
            heading_sin_total = heading_sin_total \
                - heading_sin_run[t_three]
            heading_cos_run[t_three] = math.cos(math.radians(heading))
            heading_sin_run[t_three] = math.sin(math.radians(heading))
            heading_cos_total = heading_cos_total \
                + heading_cos_run[t_three]
            heading_sin_total = heading_sin_total \
                + heading_sin_run[t_three]
            heading = round(math.degrees(math.atan2(heading_sin_total
                            / 30, heading_cos_total / 30)), 1)
            if heading < .1:
                heading = heading + 360.0

            t_damp = hack
            t_one += 1
            if t_one == 10:
                t_one = 0
            t_three += 1
            if t_three == 30:
                t_three = 0

            if hack - t_print > 0.01:			
                print("Roll : {0:.2f}\tPitch : {1:.2f}\tYaw : {2:.2f}".format(roll,pitch,heading))

                t_print = hack

        time.sleep(poll_interval * 1.0 / 1000.0)
