#!/usr/bin/python

import smbus
import math
import time
import sys
import curses
import signal
from PID import PID

screen = curses.initscr()

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

def signal_handler(signal, frame):
    curses.endwin()
    sys.exit()

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

signal.signal(signal.SIGINT, signal_handler)
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

#Set up basic screen labels 
screen.addstr(0,0,"gyro data")
screen.addstr(1,0,"---------")
screen.addstr(2,0,"gyro_xout:")
screen.addstr(2,20,"scaled:")
screen.addstr(3,0,"gyro_yout:")
screen.addstr(3,20,"scaled:")
screen.addstr(4,0,"gyro_zout:")
screen.addstr(4,20,"scaled:")
screen.addstr(6,0,"accelerometer data")
screen.addstr(7,0,"------------------")
screen.addstr(8,0,"accel_xout:")
screen.addstr(8,20,"scaled:")
screen.addstr(9,0,"accel_yout:")
screen.addstr(9,20,"scaled:")
screen.addstr(10,0,"accel_zout:")
screen.addstr(10,20,"scaled:")
screen.addstr(12,0,"x rotation:")
screen.addstr(13,0,"y rotation:")

#list of accel values for averaging
accelvalues = []

while True:
    gyro_xout = read_word_2c(0x43)
    gyro_yout = read_word_2c(0x45)
    gyro_zout = read_word_2c(0x47)
    
    #clear existing values
    screen.addstr(2,12,"        ")
    screen.addstr(2,28,"      ")
    screen.addstr(3,12,"        ")
    screen.addstr(3,28,"      ")
    screen.addstr(4,12,"        ")
    screen.addstr(4,28,"      ")
    screen.addstr(8,12,"        ")
    screen.addstr(8,28,"      ")
    screen.addstr(9,12,"        ")
    screen.addstr(9,28,"      ")
    screen.addstr(10,12,"        ")
    screen.addstr(10,28,"      ")
    screen.addstr(12,12,"                 ")
    screen.addstr(13,12,"                 ")

    screen.addstr(2,12,str(gyro_xout))
    screen.addstr(2,28,str((gyro_xout / 131)))
    screen.addstr(3,12,str(gyro_yout))
    screen.addstr(3,28,str((gyro_yout / 131)))
    screen.addstr(4,12,str(gyro_zout))
    screen.addstr(4,28,str((gyro_zout / 131)))

    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)

    accel_xout_scaled = accel_xout / 16384.0
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0

    screen.addstr(8,12,str(accel_xout))
    screen.addstr(8,28,str(accel_xout_scaled))
    screen.addstr(9,12,str(accel_yout))
    screen.addstr(9,28,str(accel_yout_scaled))
    screen.addstr(10,12,str(accel_zout))
    screen.addstr(10,28,str(accel_zout_scaled))
    
    screen.addstr(12,12,str(get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)))
    screen.addstr(13,12,str(get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)))
    
    #calculate average accel value
    accelvalues.append(get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))
    if len(accelvalues) > 10:
        del accelvalues[0]
    average = float(sum(accelvalues)) / len(accelvalues)
    
    screen.addstr(15,0,str(round(average,2)))
    screen.addstr(15,0,str(PID.GenOut(PID, 1)))
    screen.refresh()