from __future__ import print_function

import subprocess
import time

import epics
import numpy as np

from helpers import mgnt_utilities as utilities
from helpers.mgnt_facility_agent import FacilityAgent, CAException
from helpers.mgnt_utilities import is_string
from .mgnt_auto_checkout_test import AutoCheckoutTest


class PerturbTest(AutoCheckoutTest):
    test_id = 'perturb'
    description = 'Perturb'
    read_only = False
    deferred = False

    def __init__(self):
        super(PerturbTest, self).__init__(
            pre_checkout_routines=[]
        )

    def execute(self, magnet):
        # Ensure device is in service
        if utilities.results['in_service'].data != 'In-Service':
            self.status = None
            return

        # Ensure perturb is enabled
        if not utilities.results['perturb_enabled'].data.startswith('Enabled'):
            self.status = None
            return

        # Ensure that magnet is not a string or bulk
        if is_string(magnet) or magnet['psc_type'] == 'SERIAL':
            self.status = None
            return

        # Get trim tolerance
        trim_btol_pv = '{}:TRIMBTOL'.format(magnet['device'])
        trim_btol = epics.caget(trim_btol_pv)
        if trim_btol is None:
            self.data = '{} not found'.format(trim_btol_pv)
            self.status = False
            print('FAIL')
            return

        # Turn device on
        state_set_pt_pv = '{}:STATESETPT'.format(magnet['device'])
        agent = FacilityAgent(utilities.facility)
        try:
            agent.caput(state_set_pt_pv, 1)
        except (subprocess.CalledProcessError, CAException) as e:
            self.data = 'Error trying to power on {}: {}'.format(magnet['device'], e.output)
            self.status = False
            print('FAIL')
            return

        # Wait 2 seconds
        time.sleep(2)

        # Verify that device is on
        state_pv = '{}:STATE'.format(magnet['device'])
        state = epics.caget(state_pv, as_string=True)
        if state is None:
            self.data = '{} not found'.format(state_pv)
            self.status = False
            print('FAIL')
            return

        elif state != 'ON':
            self.data = '{} is expected to be on but {} is {}'.format(magnet['device'], state_pv, state)
            self.status = False
            print('FAIL')
            return

        # Save BDES
        ctrl_pv = '{}:CTRL'.format(magnet['device'])
        agent = FacilityAgent(utilities.facility)
        try:
            agent.caput(ctrl_pv, 'SAVE_BDES')
        except (subprocess.CalledProcessError, CAException) as e:
            self.data = 'Error trying to save BDES for {}: {}'.format(magnet['device'], e.output)
            self.status = False
            print('FAIL')
            return

        # Monitor BDES, BACT, IACT, and ICTRL
        fields = ['BDES', 'BACT', 'IACT', 'ICTRL']
        pvs = ['{}:{}'.format(magnet['device'], field) for field in fields]
        for pv in pvs:
            epics.camonitor(pv, callback=self.record_data)

        # Set BDES to 3 values between BMIN and BMAX
        bdes_pv = '{}:BDES'.format(magnet['device'])
        new_bdes_values = np.linspace(magnet['bmin'], magnet['bmax'], 3)
        agent = FacilityAgent(utilities.facility)
        noise_failed = False
        for new_bdes in new_bdes_values:
            # Enter value for BDES
            try:
                agent.caput(bdes_pv, new_bdes)
            except (subprocess.CalledProcessError, CAException) as e:
                for pv in pvs:
                    epics.camonitor_clear(pv)

                self.data = 'Error trying to set {} to {}: {}'.format(bdes_pv, new_bdes, e.output)
                self.status = False
                print('FAIL')
                break

            # Perturb
            ctrl_pv = '{}:CTRL'.format(magnet['device'])
            try:
                agent.caput(ctrl_pv, 'PERTURB')
            except (subprocess.CalledProcessError, CAException) as e:
                for pv in pvs:
                    epics.camonitor_clear(pv)

                self.data = 'Error trying to perturb {}: {}'.format(magnet['device'], e.output)
                self.status = False
                print('FAIL')
                break

            # Wait until perturb is done
            ctrl_state_pv = magnet['device'] + ':CTRLSTATE'
            ctrl_state = epics.caget(ctrl_state_pv, as_string=True)
            while ctrl_state != 'Done':
                print('Not done perturbing yet, waiting...')
                time.sleep(1)
                ctrl_state = epics.caget(ctrl_state_pv, as_string=True)

            # Wait until BACT settles
            bact_pv = magnet['device'] + ':BACT'
            previous = epics.caget(bact_pv)
            state = True
            counter = 0
            current = epics.caget(bact_pv)
            past_diffs = [abs(new_bdes - current)]
            while True:
                time.sleep(2)
                current = epics.caget(bact_pv)
                moving = abs(current - previous) > trim_btol
                curr_diff = abs(new_bdes - current)
                past_diffs.append(curr_diff)
                if moving != state:
                    counter += 1
                    # Check if we are continually getting closer to BDES after 10 seconds
                    if counter >= 5:
                        if curr_diff <= trim_btol or curr_diff >= past_diffs[0]:
                            print('Done')
                            break

                        past_diffs = past_diffs[1:]
                        print('Approaching very slowly...')

                    else:
                        print('Slowing down...')

                else:
                    # Special case: stop if we are hovering around BDES but are still out of tolerance
                    if len(past_diffs) > 0 and np.count_nonzero(np.diff(np.sign(np.diff(past_diffs[-10:])))) >= 5:
                        for pv in pvs:
                            epics.camonitor_clear(pv)

                        self.data = '{} is noisy after perturbing to {}'.format(bact_pv, new_bdes)
                        self.status = False
                        print('FAIL')
                        noise_failed = True
                        break

                    counter = 0
                    print('Still moving, waiting...')

                previous = current

            # Abort if test already failed due to noisiness
            if noise_failed:
                break

            # Make sure status is not out of tolerance
            # Since we are only perturbing and not trimming, loosen the tolerance by a factor of 10
            bact = epics.caget(bact_pv)
            if abs(bact - new_bdes) > 10 * trim_btol:
                for pv in pvs:
                    epics.camonitor_clear(pv)

                self.data = '{} is {} after perturbing to {} (out of tolerance)'.format(bact_pv, bact, new_bdes)
                self.status = False
                print('FAIL')
                break

        # Stop monitoring BDES, BACT, IACT, and ICTRL
        for pv in pvs:
            epics.camonitor_clear(pv)

        # Load BDES
        try:
            agent.caput(ctrl_pv, 'LOAD_BDES')
        except (subprocess.CalledProcessError, CAException) as e:
            self.data = 'Error trying to load BDES for {}: {}'.format(magnet['device'], e.output)
            self.status = False
            print('FAIL')

        # Perturb
        ctrl_pv = '{}:CTRL'.format(magnet['device'])
        try:
            agent.caput(ctrl_pv, 'PERTURB')
        except (subprocess.CalledProcessError, CAException) as e:
            for pv in pvs:
                epics.camonitor_clear(pv)

            self.data = 'Error trying to perturb {}: {}'.format(magnet['device'], e.output)
            self.status = False
            print('FAIL')

        # Wait until perturb is done
        ctrl_state_pv = magnet['device'] + ':CTRLSTATE'
        ctrl_state = epics.caget(ctrl_state_pv, as_string=True)
        while ctrl_state != 'Done':
            print('Not done perturbing yet, waiting...')
            time.sleep(1)
            ctrl_state = epics.caget(ctrl_state_pv, as_string=True)

        # Do nothing else on failure
        if self.status is not False:
            self.data = 'Successfully perturbed {} to {}'.format(bdes_pv, new_bdes_values)
            self.status = True
            print('PASS')
