#!/usr/bin/python
#
#    This program  reads the angles from the acceleromteer, gyroscope
#    and mangnetometer on a BerryIMU connected to a Raspberry Pi.
#
#    This program includes two filters (low pass and median) to improve the
#    values returned from BerryIMU by reducing noise.
#
#    The BerryIMUv1, BerryIMUv2 and BerryIMUv3 are supported
#
#    This script is python 2.7 and 3 compatible
#
#    Feel free to do whatever you like with this code.
#    Distributed as-is; no warranty is given.
#
#    http://ozzmaker.com/

import sys
import time
import random
import math
import IMU
import datetime
import os

#fuck u Jona u piece of shite

RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070          # [deg/s/LSB]  If you change the dps for gyro, you need to update this value accordingly
AA =  0.40              # Complementary filter constant
MAG_LPF_FACTOR = 0.4    # Low pass filter constant magnetometer
ACC_LPF_FACTOR = 0.4    # Low pass filter constant for accelerometer
ACC_MEDIANTABLESIZE = 9         # Median filter table size for accelerometer. Higher = smoother but a longer delay
MAG_MEDIANTABLESIZE = 9         # Median filter table size for magnetometer. Higher = smoother but a longer delay

GYRO_X_LIFT = 50
GYRO_Y_LIFT = 40
GYRO_Z_LIFT = 40

GYRO_X_TWIST = 50
GYRO_Y_TWIST = 110
GYRO_Z_TWIST = 50

GYRO_X_CHOP = 40
GYRO_Y_CHOP = 40
GYRO_Z_CHOP = 50

################# Compass Calibration values ############
# Use calibrateBerryIMU.py to get calibration values
# Calibrating the compass isnt mandatory, however a calibrated
# compass will result in a more accurate heading value.

magXmin =  0
magYmin =  0
magZmin =  0
magXmax =  0
magYmax =  0
magZmax =  0

'''
Here is an example:
magXmin =  -1748
magYmin =  -1025
magZmin =  -1876
magXmax =  959
magYmax =  1651
magZmax =  708
Dont use the above values, these are just an example.
'''
############### END Calibration offsets #################

# Filter variables
gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
oldXMagRawValue = 0
oldYMagRawValue = 0
oldZMagRawValue = 0
oldXAccRawValue = 0
oldYAccRawValue = 0
oldZAccRawValue = 0

a = datetime.datetime.now()

# Setup the tables for the mdeian filter. Fill them all with '1' so we dont get devide by zero error
acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE
acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE
acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE
acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE
mag_medianTable1X = [1] * MAG_MEDIANTABLESIZE
mag_medianTable1Y = [1] * MAG_MEDIANTABLESIZE
mag_medianTable1Z = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2X = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2Y = [1] * MAG_MEDIANTABLESIZE
mag_medianTable2Z = [1] * MAG_MEDIANTABLESIZE

IMU.detectIMU()     # Detect if BerryIMU is connected.
if(IMU.BerryIMUversion == 99):
    print(" No BerryIMU found... exiting ")
    sys.exit()
IMU.initIMU()       # Initialise the accelerometer, gyroscope and compass

gyrox_list = [0] * 5
gyroy_list = [0] * 5
gyroz_list = [0] * 5

clf_action = "None"
action_list = ["None"] * 5

while True:
    # Read the accelerometer,gyroscope and magnetometer values
    ACCx = IMU.readACCx()
    ACCy = IMU.readACCy()
    ACCz = IMU.readACCz()
    GYRx = IMU.readGYRx()
    GYRy = IMU.readGYRy()
    GYRz = IMU.readGYRz()
    MAGx = IMU.readMAGx()
    MAGy = IMU.readMAGy()
    MAGz = IMU.readMAGz()

    # Apply compass calibration
    MAGx -= (magXmin + magXmax) / 2
    MAGy -= (magYmin + magYmax) / 2
    MAGz -= (magZmin + magZmax) / 2


    # Calculate loop Period(LP). How long between Gyro Reads
    b = datetime.datetime.now() - a
    a = datetime.datetime.now()
    LP = b.microseconds/(1000000*1.0)
    outputString = "Loop Time %5.2f " % ( LP )

    ###############################################
    #### Apply low pass filter ####
    ###############################################
    MAGx =  MAGx  * MAG_LPF_FACTOR + oldXMagRawValue*(1 - MAG_LPF_FACTOR);
    MAGy =  MAGy  * MAG_LPF_FACTOR + oldYMagRawValue*(1 - MAG_LPF_FACTOR);
    MAGz =  MAGz  * MAG_LPF_FACTOR + oldZMagRawValue*(1 - MAG_LPF_FACTOR);
    ACCx =  ACCx  * ACC_LPF_FACTOR + oldXAccRawValue*(1 - ACC_LPF_FACTOR);
    ACCy =  ACCy  * ACC_LPF_FACTOR + oldYAccRawValue*(1 - ACC_LPF_FACTOR);
    ACCz =  ACCz  * ACC_LPF_FACTOR + oldZAccRawValue*(1 - ACC_LPF_FACTOR);

    oldXMagRawValue = MAGx
    oldYMagRawValue = MAGy
    oldZMagRawValue = MAGz
    oldXAccRawValue = ACCx
    oldYAccRawValue = ACCy
    oldZAccRawValue = ACCz

    #########################################
    #### Median filter for accelerometer ####
    #########################################
    # cycle the table
    for x in range (ACC_MEDIANTABLESIZE-1,0,-1 ):
        acc_medianTable1X[x] = acc_medianTable1X[x-1]
        acc_medianTable1Y[x] = acc_medianTable1Y[x-1]
        acc_medianTable1Z[x] = acc_medianTable1Z[x-1]

    # Insert the lates values
    acc_medianTable1X[0] = ACCx
    acc_medianTable1Y[0] = ACCy
    acc_medianTable1Z[0] = ACCz

    # Copy the tables
    acc_medianTable2X = acc_medianTable1X[:]
    acc_medianTable2Y = acc_medianTable1Y[:]
    acc_medianTable2Z = acc_medianTable1Z[:]

    # Sort table 2
    acc_medianTable2X.sort()
    acc_medianTable2Y.sort()
    acc_medianTable2Z.sort()

    # The middle value is the value we are interested in
    ACCx = acc_medianTable2X[int(ACC_MEDIANTABLESIZE/2)];
    ACCy = acc_medianTable2Y[int(ACC_MEDIANTABLESIZE/2)];
    ACCz = acc_medianTable2Z[int(ACC_MEDIANTABLESIZE/2)];

    #########################################
    #### Median filter for magnetometer ####
    #########################################
    # cycle the table
    for x in range (MAG_MEDIANTABLESIZE-1,0,-1 ):
        mag_medianTable1X[x] = mag_medianTable1X[x-1]
        mag_medianTable1Y[x] = mag_medianTable1Y[x-1]
        mag_medianTable1Z[x] = mag_medianTable1Z[x-1]

    # Insert the latest values
    mag_medianTable1X[0] = MAGx
    mag_medianTable1Y[0] = MAGy
    mag_medianTable1Z[0] = MAGz

    # Copy the tables
    mag_medianTable2X = mag_medianTable1X[:]
    mag_medianTable2Y = mag_medianTable1Y[:]
    mag_medianTable2Z = mag_medianTable1Z[:]

    # Sort table 2
    mag_medianTable2X.sort()
    mag_medianTable2Y.sort()
    mag_medianTable2Z.sort()

    # The middle value is the value we are interested in
    MAGx = mag_medianTable2X[int(MAG_MEDIANTABLESIZE/2)];
    MAGy = mag_medianTable2Y[int(MAG_MEDIANTABLESIZE/2)];
    MAGz = mag_medianTable2Z[int(MAG_MEDIANTABLESIZE/2)];

    # Convert Gyro raw to degrees per second
    rate_gyr_x =  GYRx * G_GAIN
    rate_gyr_y =  GYRy * G_GAIN
    rate_gyr_z =  GYRz * G_GAIN

    # Calculate the angles from the gyro.
    gyroXangle+=rate_gyr_x*LP
    gyroYangle+=rate_gyr_y*LP
    gyroZangle+=rate_gyr_z*LP

    ######################## START Thresholding #########################

    gyrox_list = [gyroXangle] + gyrox_list
    gyrox_list.pop()

    gyroy_list = [gyroYangle] + gyroy_list
    gyroy_list.pop()

    gyroz_list = [gyroZangle] + gyroz_list
    gyroz_list.pop()

    max_gyrox = max(gyrox_list)
    max_gyrox_index = gyrox_list.index(max_gyrox)
    min_gyrox = min(gyrox_list)
    min_gyrox_index = gyrox_list.index(min_gyrox)

    max_gyroy = max(gyroy_list)
    max_gyroy_index = gyroy_list.index(max_gyroy)
    min_gyroy = min(gyroy_list)
    min_gyroy_index = gyroy_list.index(min_gyroy)

    max_gyroz = max(gyroz_list)
    max_gyroz_index = gyroz_list.index(max_gyroz)
    min_gyroz = min(gyroz_list)
    min_gyroz_index = gyroz_list.index(min_gyroz)

    max_gyrox_diff = max_gyrox - min_gyrox
    max_gyroy_diff = max_gyroy - min_gyroy
    max_gyroz_diff = max_gyroz - min_gyroz

    if max_gyrox_diff > GYRO_X_LIFT and max_gyroy_diff < GYRO_Y_LIFT and max_gyroz_diff < GYRO_Z_LIFT:
        if min_gyrox_index < max_gyrox_index and action_list.count("None") > 4:
            clf_action = "Upward_Lift"
            # print(max_gyrox_diff, max_gyroy_diff, max_gyroz_diff)
        else:
            clf_action = "None"
    elif max_gyroy_diff > GYRO_Y_TWIST and max_gyrox_diff < GYRO_X_TWIST and max_gyroz_diff < GYRO_Z_TWIST: 
        if min_gyroy_index < max_gyroy_index and action_list.count("None") > 4:
            clf_action = "Clockwise_Twist"
            # print(max_gyrox_diff, max_gyroy_diff, max_gyroz_diff)
        else:
            clf_action = "None"
    elif max_gyroz_diff > GYRO_Z_CHOP and max_gyrox_diff < GYRO_X_CHOP and max_gyroy_diff < GYRO_Y_CHOP:
        if min_gyroz_index < max_gyroz_index and action_list.count("None") > 4:
            clf_action = "Vertical_Chop"
            # print(max_gyrox_diff, max_gyroy_diff, max_gyroz_diff)
        else:
            clf_action = "None"
    else:
        clf_action = "None"

    action_list = [clf_action] + action_list
    action_list.pop()

    print("Classifier action:", clf_action)

    if clf_action is "Upward_Lift":
    	for i in range(5):
            gyrox_list = [gyroXangle] + gyrox_list
            gyrox_list.pop()

    if clf_action is "Clockwise_Twist":
    	for i in range(5):
            gyroy_list = [gyroYangle] + gyroy_list
            gyroy_list.pop()

    if clf_action is "Vertical_Chop":
    	for i in range(5):
            gyroz_list = [gyroZangle] + gyroz_list
            gyroz_list.pop()

    time.sleep(0.05)
