# -*- coding: utf-8 -*-
'''Polarimeter is the driver for the polarimeter device.

It provides public methods to steer hardware (light, analyzer wheel, shutdown),
read data from the sensor and start higher level automated actions,
 like calibration and determination of rotation angle versus zero.
It utilizes file 'offsets.txt' to persist position, zero angle offset and high
 and low values between program runs, default content is "0 0 0 0".   

The algorithms for finding the maximum (extinction) value use a second degree
polynomial regression and a kind of hillclimbing algorithm to get close to the peak.


@author: guido lutterbach
@contact: guido@smartypies.com
@version: 1.0
@copyright: guido lutterbach, July 2018
'''

import time
import spidev
import RPi.GPIO as GPIO
import numpy
import logging
from math import copysign
from statistics import mean


class Polarimeter:
    '''Class for low level, device access functionality. External calls have
     to have correct arguments, no exception handling here!
    '''
    # light driver
    __lightchannel = 21
    __lighton = False

    # motor driver
    __motorchannels = [26, 19, 13, 6]
    __stepseqence = [[1, 0, 1, 0], [0, 0, 1, 0], [0, 1, 1, 0], [0, 1, 0, 0],
                     [0, 1, 0, 1], [0, 0, 0, 1], [1, 0, 0, 1], [1, 0, 0, 0]]
    __stepcount = len(__stepseqence)
    __stepdirection = 1
    __stepseqposition = 0

    # SPI driver
    __spi = spidev.SpiDev()
    __measurechannel = 0
    __highvalue = -1
    # max value from A/D chip, e.g. 4095 for MCP3208
    __maxadvalue = 4095
    __lowvalue = __maxadvalue + 1
    __valuerange = -1
    __speed_hz = 250000
    __delay_usec = 250

    # position and geometry
    __position = 0
    __offset = 0.0
    __peakwidth = 31
    __fullrotationsteps = 3600
    __stepsperdegree = int(__fullrotationsteps /
                           360)  # preferably >5, needed for search step width

    # tunable parameters
    __samplingcount = 20
    __movewaittime = 0.03
    __measurewaittime = 0.02

    def __init__(self):
        '''Init hardware and reset variables.'''
        self.logger = logging.getLogger('Polarimeter')
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.__lightchannel, GPIO.OUT)
        self.lightoff()
        GPIO.setup(self.__motorchannels, GPIO.OUT)
        self.motoroff()
        self.__spi.open(0, 0)
        self.__spi.max_speed_hz = self.__speed_hz
        self.__setstatus()
        return

    def lightoff(self):
        '''Turn light off.'''
        self.__lighton = False
        GPIO.output(self.__lightchannel, False)

    def lighton(self):
        '''Turn light on.'''
        self.__lighton = True
        GPIO.output(self.__lightchannel, True)

    def motoroff(self):
        '''Move to stable full step position, if necessary and set pins to off.'''
        if self.__stepseqposition % 2 != 0:
            self.forward()
        GPIO.output(self.__motorchannels, False)

    def forward(self):
        '''Set direction formward and move one step.'''
        self.__stepdirection = 1
        self.__move()

    def backward(self):
        '''Set direction backward and move one step.'''
        self.__stepdirection = -1
        self.__move()

    def measure(self):
        '''Read value(s) from A/D chip, also return position and timestamp.'''
        data = Measurement(pos=self.__position)
        time.sleep(self.__measurewaittime)
        val = []
        for iter in range(0, self.__samplingcount):
            val.append(self.__readchannel(self.__measurechannel))
        data.value = mean(val)
        data.markset = self.__setmarks(data.value)
        return data

    def gotoposition(self, position):
        '''Rotate to position. Any integer value gets converted to [0,3600[.'''
        self.logger.info('gotoposition from {} to {}'.format(
            self.__position, position))
        position %= self.__fullrotationsteps
        position = int(position)
        if self.__position == position:
            return
        elif self.__position > position:
            distance = self.__fullrotationsteps - self.__position + position
        else:
            distance = position - self.__position
        # if distance <= 1800 walk along distance
        if abs(distance) <= self.__fullrotationsteps / 2:
            self.__stepdirection = int(copysign(1, distance))
        else:
            # otherwise switch direction
            self.__stepdirection = int(copysign(1, distance * -1))
        while self.__position != position:
            self.__move()

    def setzero(self):
        '''Find maximum value and set position as zero position.'''
        self.__highvalue = -1
        self.__lowvalue = self.__maxadvalue + 1
        self.__valuerange = -1
        startpoint = self.__scanscene()
        self.gotoposition(startpoint.position)
        vicinity = self.__approachpeak(startpoint)
        try:
            self.__offset = self.findmax(vicinity)
            # set position at the end of the regression as  0, for orientation
            # It is not the peak itself, but close (peakwidth/2)
            # steps after the peak, see findmax.
            self.__position = 0
            self.motoroff()
        except Exception as ex:
            self.logger.error('Setting to zero failed! Please retry!')

    def analyze(self):
        '''Find maximum value (=minimum light), by rotation of analyzer and 
        return angle with respect to zero position.

        If a maximum was not found, an error is written to console and 
        -4711 is returned.'''
        startpoint = self.measure()
        vicinity = self.__approachpeak(startpoint)
        try:
            currentoffset = self.findmax(vicinity)
        except Exception:
            self.logger.error('Finding rotation angle failed! Please retry!')
            return -4711

        deltamax = currentoffset - self.__offset
        deltaangle = (
            self.__position + deltamax) * 360 / self.__fullrotationsteps
        # turn motor off
        self.motoroff()
        # transform into value from -90 to +90
        if deltaangle > 270:
            deltaangle -= 360
        elif deltaangle > 90:
            deltaangle -= 180
        self.logger.info('analyze: value {}'.format(deltaangle))
        return '{:.2f}'.format(deltaangle)

    def __approachpeak(self, startpoint):
        '''Return point close to or at the peak.

        Walk from point A in direction of slope (+ or -) search radius steps
        to point B. If slopes in A an B point have the same sign, continue
        in this direction with B as new A. Otherwise the peak is in between, 
        set middle of A and B as new A. Stop, if slope in A or B is small
        AND within top 5% of values or if slopes have different signs AND search
        radius is small.'''

        pointA = startpoint
        slope = None

        while True:
            searchradius = self.__searchradius(pointA)
            #determine direction of walk
            if slope is None:
                slope = self.__calculateslope(pointA)

            self.logger.info(
                'approachpeak: position {} searchradius {} slope {}'.format(
                    pointA.position, searchradius, slope))

            if abs(slope) <= 0.5:
                if self.__valueintoppercent(pointA.value, 5):
                    # small slope and in top 5 % => at the peak
                    self.logger.info(
                        'return point A: {}, {}, slope= {}'.format(
                            pointA.position, pointA.value, slope))
                    return pointA
                elif not self.__valueintoppercent(pointA.value, 95):
                    # small slope and in lower 5% => valley. bias slope in one direction
                    slope = copysign(slope, 1)
            else:
                # larger slopes are generally safe to move along and for analysis
                pass
            searchradius = copysign(searchradius, slope)

            # assess point B
            self.gotoposition(pointA.position + searchradius)
            pointB = self.measure()
            slopeA = slope  # remember slope in A
            slope = self.__calculateslope(pointB)
            self.logger.info('pointB: position {} slope {}'.format(
                pointB.position, slope))
            if abs(slope) <= 0.5:
                # small slope and in top 5 % => at the peak
                if self.__valueintoppercent(pointB.value, 5):
                    self.logger.info(
                        'return point B: {}, {}, slope= {}'.format(
                            pointB.position, pointB.value, slope))
                    return pointB
                elif not self.__valueintoppercent(pointB.value, 95):
                    # small slope and in lower 5% => valley. bias slope in one direction
                    slope = copysign(slope, 1)
            else:
                # larger slopes are generally safe to move along and for analysis
                pass

            if slopeA * slope > 0:
                # continue in this direction with B as new A, keep slope
                pointA = pointB
            else:
                # one positive slope and one negative slope means that the peak is in between
                self.gotoposition(
                    self.__middle(pointA.position, pointB.position))
                pointA = self.measure()
                if abs(searchradius) <= self.__peakwidth / 2:
                    self.logger.info(
                        'slopes with different signs and small search radius, return the middle point: {}, {} '.
                        format(pointA.position, pointA.value))
                    return pointA
                else:
                    self.logger.info(
                        'slopes with different signs, continue with middle point: {}, {} '.
                        format(pointA.position, pointA.value))
                    slope = None

    def __searchradius(self, point):
        '''Depending on the point's position in the observed range of values,
         the search radius is adjusted.

        The value ranges and the radiuses are selected to assure that with the
         resulting radius a range can be left with a few steps, but also that 
         regions with larger values are not left out. In other words:
         small value => big radius, big value => small radius.'''
        if not self.__valueintoppercent(point.value, 60):
            # far off
            return 20 * self.__stepsperdegree
        elif not self.__valueintoppercent(point.value, 35):
            return 5 * self.__stepsperdegree
        elif not self.__valueintoppercent(point.value, 10):
            return 2 * self.__stepsperdegree
        elif not self.__valueintoppercent(point.value, 5):
            return 1 * self.__stepsperdegree
        else:
            # 5 steps as the smallest step width into the peak
            return 5

    def __valueintoppercent(self, value, percent):
        '''Return True if the value in the top percent (0-100) of value range,
         False otherwise.'''
        diff = abs(value - self.__lowvalue)
        return diff >= self.__valuerange * (100 - percent) / 100

    def __middle(self, pos0, pos1):
        '''calculate the smaller middle position between 2 points on the circle.'''
        # normalize
        pos0n = pos0 % self.__fullrotationsteps
        pos1n = pos1 % self.__fullrotationsteps
        m = int(pos0n + pos1n) / 2
        if abs(pos0n - pos1n) > self.__fullrotationsteps / 2:
            m = (m + self.__fullrotationsteps / 2) % self.__fullrotationsteps
        return int(m)

    def __distance(self, pos0, pos1):
        '''Helper function to calulate the distance between 2 positions,
         especially when comparing values around zero.'''
        diff = abs(pos0 - pos1)
        if diff > self.__fullrotationsteps / 2:
            return self.__fullrotationsteps - diff
        else:
            return diff

    def __scanscene(self):
        '''Get an overview about value "landscape" and return the max. position
         for Hill-Climber search.

        This method should be called at the beginning and only once for zero calibration.
        '''
        # 5deg will get close to the peak
        scanstep = int(5 * self.__fullrotationsteps / 360)  #5
        countscan = 36
        topvalueindex = -1
        scanvalues = []
        for i in range(0, countscan):
            self.gotoposition(self.__position + scanstep)
            currentvalue = self.measure()
            scanvalues.append(currentvalue)
            if currentvalue.markset > 0:
                topvalueindex = i
        return scanvalues[topvalueindex]

    def __calculateslope(self, searchpoint):
        '''Approximate slope.'''
        # slope delta, 3 seems to work fine
        slopedelta = 3
        self.gotoposition(searchpoint.position + slopedelta)
        return (self.measure().value - searchpoint.value) / slopedelta

    def __setmarks(self, value):
        '''Set high and low marks and inform about what was set.

        gotcha: first value coming in has to set both'''
        if self.__highvalue < 0:
            self.__highvalue = value
            self.__lowvalue = value
            self.__valuerange = 0
            return 2

        if value > self.__highvalue:
            self.__highvalue = value
            self.__valuerange = self.__highvalue - self.__lowvalue
            return 1
        elif value < self.__lowvalue:
            self.__lowvalue = value
            self.__valuerange = self.__highvalue - self.__lowvalue
            return -1
        else:
            return 0

    def __move(self):
        '''Move one increment. include wait here to prevent gear slide.'''
        time.sleep(self.__movewaittime)
        self.__position += self.__stepdirection
        # keep __position within [0, 3600[
        self.__position += self.__fullrotationsteps
        self.__position %= self.__fullrotationsteps

        self.__stepseqposition += self.__stepdirection
        self.__stepseqposition %= self.__stepcount

        for pin in range(0, 4):
            if self.__stepseqence[self.__stepseqposition][pin] != 0:
                GPIO.output(self.__motorchannels[pin], True)
            else:
                GPIO.output(self.__motorchannels[pin], False)

    def __setstatus(self):
        '''Read position, offset, high and low values and value range from file.'''
        with open('offsets.txt', 'r') as f:
            pos, oz, hv, lv = f.readline().split()
            self.__position = int(pos)
            self.__offset = float(oz)
            self.__highvalue = float(hv)
            self.__lowvalue = float(lv)
            self.__valuerange = self.__highvalue - self.__lowvalue

    def __readchannel(self, channel):
        '''Read data from AD Chip via SPI.'''
        # experiment to find suitable  parameters with tune
        adc = self.__spi.xfer2([6 | (channel & 4) >> 2, (channel & 3) << 6, 0],
                               self.__speed_hz, self.__delay_usec)
        data = ((adc[1] & 15) << 8) + adc[2]
        return data

    def tune(self, sc, mw, rw):
        '''Tune parameters.

        usage: tune samplingcount measurewait rotatewait
            all parameters are required'''
        self.logger.info('tune values before: ' + ','.join(
            str(x) for x in (self.__samplingcount, self.__measurewaittime,
                             self.__movewaittime)))
        self.__samplingcount = sc
        self.__measurewaittime = mw
        self.__movewaittime = rw

    def shutdown(self):
        '''Rotate to motor position 0, safe offsets,
        turn off lights and motor, clean GPIO, close SPI.'''
        while self.__stepseqposition != 0:
            self.forward()
        with open('offsets.txt', 'w') as f:
            f.write("{} {} {} {}".format(self.__position, self.__offset,
                                         self.__highvalue, self.__lowvalue))
        self.lightoff()
        self.motoroff()
        GPIO.cleanup()
        self.__spi.close()

    def findmax(self, vicinity=None):
        '''Measure 'enough' points before and after the  peak for second
         order regression.
        
        Do not move before evaluating result, as it depends on current
         self.__position.'''
        if vicinity is None:
            vicinity = self.measure()
        width = self.__peakwidth
        width_2 = int(width / 2)
        values = []
        self.logger.info('findmax from position {}'.format(vicinity.position))

        go = True
        cnt = 0
        while go:
            cnt += 1
            # try it max 3 times
            if cnt == 4:
                raise Exception('No peak found!')
            self.gotoposition(vicinity.position - cnt * width - 5)  # 5 extra
            currentmax = -1.0
            currentmaxposition = -1
            while go:
                self.forward()
                v = float(self.measure().value)
                values.append(v)
                if v >= currentmax:
                    currentmax = v
                    currentmaxposition = self.__position
                elif v < currentmax:
                    # we have at least width/2 values which are smaller than max now
                    if self.__distance(self.__position,
                                       currentmaxposition) >= width_2:
                        if len(values) >= width:
                            # were done
                            go = False
                        else:
                            # give it another try, because fewer values before max than after max may cause bad regression
                            values = []
                            break

        listY = values[-width:]
        listX = range(-width_2, width_2 + 1)
        polycoeffs = numpy.polyfit(listX, listY, 2)
        #derivative of x_max = 0
        maxX = -polycoeffs[1] / (polycoeffs[0] * 2)
        return maxX


class Measurement():
    '''Data class to link position and value with time.
    
    Optional attribute marks is used during scan of value range.'''

    def __init__(self, pos=0, val=0, marks=0):
        self.position = pos
        self.value = val
        # indicates if a high or low mark was set with 2,1,0,-1
        self.markset = marks
        self.time = time.time()