#!/usr/bin/python
"""
@brief QuickBot class for Beaglebone Black

@author Rowland O'Flaherty (rowlandoflaherty.com)
@date 02/07/2014
@version: 1.0
@copyright: Copyright (C) 2014, Georgia Tech Research Corporation
see the LICENSE file included with this software (see LINENSE file)
"""

from __future__ import division
import sys
import time
import re
import socket
import threading
import numpy as np

import Adafruit_BBIO.GPIO as GPIO
import Adafruit_BBIO.PWM as PWM
import Adafruit_BBIO.ADC as ADC

# Constants
LEFT = 0
RIGHT = 1
MIN = 0
MAX = 1

DEBUG = False

ADCTIME = 0.001

## Tic toc constants
TICTOC_START = 0
TICTOC_COUNT = 0
TICTOC_MEAN = 0
TICTOC_MAX = -float('inf')
TICTOC_MIN = float('inf')

## Encoder buffer constants and variables
ENC_BUF_SIZE = 2**9
ENC_IND = [0, 0]
ENC_TIME = [[0]*ENC_BUF_SIZE, [0]*ENC_BUF_SIZE]
ENC_VAL = [[0]*ENC_BUF_SIZE, [0]*ENC_BUF_SIZE]

ADC_LOCK = threading.Lock()

## Run variables
RUN_FLAG = True
RUN_FLAG_LOCK = threading.Lock()


class QuickBot():
    """The QuickBot Class"""

    # === Class Properties ===
    # Parameters
    sampleTime = 20.0 / 1000.0

    # Pins
    ledPin = 'USR1'

    # Motor Pins -- (LEFT, RIGHT)
    dir1Pin = ('P8_14', 'P8_12')
    dir2Pin = ('P8_16', 'P8_10')
    pwmPin = ('P9_16', 'P9_14')

    # ADC Pins
    irPin = ('P9_38', 'P9_40', 'P9_36', 'P9_35', 'P9_33')
    encoderPin = ('P9_39', 'P9_37')

    # Encoder counting parameter and variables
    ticksPerTurn = 16  # Number of ticks on encoder disc
    encWinSize = 2**5  # Should be power of 2
    minPWMThreshold = [45, 45]  # Threshold on the minimum value to turn wheel
    encTPrev = [0.0, 0.0]
    encThreshold = [0.0, 0.0]
    encTickState = [0, 0]
    encTickStateVec = np.zeros((2, encWinSize))

    # Constraints
    pwmLimits = [-100, 100]  # [min, max]

    # State PWM -- (LEFT, RIGHT)
    pwm = [0, 0]

    # State IR
    irVal = [0.0, 0.0, 0.0, 0.0, 0.0]
    ithIR = 0

    # State Encoder
    encTime = [0.0, 0.0]  # Last time encoders were read
    encPos = [0.0, 0.0]  # Last encoder tick position
    encVel = [0.0, 0.0]  # Last encoder tick velocity

    # Encoder counting parameters
    encCnt = 0  # Count of number times encoders have been read
    encSumN = [0, 0]  # Sum of total encoder samples
    encBufInd0 = [0, 0]  # Index of beginning of new samples in buffer
    encBufInd1 = [0, 0]  # Index of end of new samples in buffer
    encTimeWin = np.zeros((2, encWinSize))  # Moving window of encoder sample times
    encValWin = np.zeros((2, encWinSize))  # Moving window of encoder raw sample values
    encPWMWin = np.zeros((2, encWinSize))  # Moving window corresponding PWM input values
    encTau = [0.0, 0.0]  # Average sampling time of encoders

    ## Stats of encoder values while input = 0 and vel = 0
    encZeroCntMin = 2**4  # Min number of recorded values to start calculating stats
    encZeroMean = [0.0, 0.0]
    encZeroVar = [0.0, 0.0]
    encZeroCnt = [0, 0]
    encHighCnt = [0, 0]
    encLowCnt = [0, 0]
    encLowCntMin = 2

    # Variables
    ledFlag = True
    cmdBuffer = ''

    # UDP
    baseIP = '192.168.7.1'
    robotIP = '192.168.7.2'
    port = 5005
    robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    robotSocket.setblocking(False)

    # === Class Methods ===
    # Constructor
    def __init__(self, baseIP, robotIP):

        # Initialize GPIO pins
        GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT)
        GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT)
        GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT)
        GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT)

        GPIO.setup(self.ledPin, GPIO.OUT)

        # Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0)
        PWM.start(self.pwmPin[LEFT], 0)
        PWM.start(self.pwmPin[RIGHT], 0)

        # Set motor speed to 0
        self.setPWM([0, 0])

        # Initialize ADC
        ADC.setup()
        self.encoderRead = encoderRead(self.encoderPin)

        # Set IP addresses
        self.baseIP = baseIP
        self.robotIP = robotIP
        self.robotSocket.bind((self.robotIP, self.port))

        
        if DEBUG:
            ## Stats of encoder values while moving -- high, low, and all tick state
            self.encHighLowCntMin = 2**5  # Min number of recorded values to start calculating stats
            self.encHighMean = [0.0, 0.0]
            self.encHighVar = [0.0, 0.0]
            self.encHighTotalCnt = [0, 0]
        
            self.encLowMean = [0.0, 0.0]
            self.encLowVar = [0.0, 0.0]
            self.encLowTotalCnt = [0, 0]
        
            self.encNonZeroCntMin = 2**5
            self.encNonZeroMean = [0.0, 0.0]
            self.encNonZeroVar = [0.0, 0.0]
            self.encNonZeroCnt = [0, 0]

            # Record variables
            self.encRecSize = 2**13
            self.encRecInd = [0, 0]
            self.encTimeRec = np.zeros((2, self.encRecSize))
            self.encValRec = np.zeros((2, self.encRecSize))
            self.encPWMRec = np.zeros((2, self.encRecSize))
            self.encNNewRec = np.zeros((2, self.encRecSize))
            self.encPosRec = np.zeros((2, self.encRecSize))
            self.encVelRec = np.zeros((2, self.encRecSize))
            self.encTickStateRec = np.zeros((2, self.encRecSize))
            self.encThresholdRec = np.zeros((2, self.encRecSize))

    # Getters and Setters
    def setPWM(self, pwm):
        # [leftSpeed, rightSpeed]: 0 is off, caps at min and max values

        self.pwm[LEFT] = min(
  
          max(pwm[LEFT], self.pwmLimits[MIN]), self.pwmLimits[MAX])
        self.pwm[RIGHT] = min(
            max(pwm[RIGHT], self.pwmLimits[MIN]), self.pwmLimits[MAX])

        # Left motor
        if self.pwm[LEFT] > 0:
            GPIO.output(self.dir1Pin[LEFT], GPIO.LOW)
            GPIO.output(self.dir2Pin[LEFT], GPIO.HIGH)
            PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT]))
        elif self.pwm[LEFT] < 0:
            GPIO.output(self.dir1Pin[LEFT], GPIO.HIGH)
   
         GPIO.output(self.dir2Pin[LEFT], GPIO.LOW)
            PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT]))
        else:
            GPIO.output(self.dir1Pin[LEFT], GPIO.LOW)
            GPIO.output(self.dir2Pin[LEFT], GPIO.LOW)
            PWM.set_duty_cycle(self.pwmPin[LEFT], 0)

        # Right motor
        if self.pwm[RIGHT] > 0:
            GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW)
            GPIO.output(self.dir2Pin[RIGHT], GPIO.HIGH)
            PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT]))
        elif self.pwm[RIGHT] < 0:
            GPIO.output(self.dir1Pin[RIGHT], GPIO.HIGH)
            GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW)
            PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT]))
        else:
            GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW)
            GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW)
            PWM.set_duty_cycle(self.pwmPin[RIGHT], 0)

    # Methods
    def run(self):
        global RUN_FLAG
        self.encoderRead.start()
        try:
            while RUN_FLAG is True:
                self.update()

                # Flash BBB LED
                if self.ledFlag is True:
                    self.ledFlag = False
                    GPIO.output(self.ledPin, GPIO.HIGH)
                else:
                    self.ledFlag = True
                    GPIO.output(self.ledPin, GPIO.LOW)
                time.sleep(self.sampleTime)
        except:
            RUN_FLAG_LOCK.acquire()
            RUN_FLAG = False
            RUN_FLAG_LOCK.release()
            raise

        self.cleanup()
        return

    def cleanup(self):
        sys.stdout.write("Shutting down...")
        self.setPWM([0, 0])
        self.robotSocket.close()
        GPIO.cleanup()
        PWM.cleanup()
        if DEBUG:
            # tictocPrint()
            self.writeBuffersToFile()
        sys.stdout.write("Done\n")

    def update(self):
        self.readIRValues()
        self.readEncoderValues()
        self.parseCmdBuffer()

    def parseCmdBuffer(self):
        global RUN_FLAG
        try:
            line = self.robotSocket.recv(1024)
        except socket.error as msg:
            return

        self.c