from subsystems import Subsystem
import logging
import time

class Focus(Subsystem):        
    def before_exposure(self, exposure, fitsbuilder):
        focoffset = exposure['FOCOFFSET']
        band = exposure['FILTER']
        target = exposure['TARGET']
        MOUNTEMP = float(self.server.get_temperature())
        if focoffset != 'IDLE':
            if focoffset != 'ADJUST':
                self.current_offset = int(focoffset)
            focus = self.focus_model(self.current_offset, band, MOUNTEMP, target)
            self.move(focus)
            focusenc = self.server.current_position()            
        else:
            focus = self.focus_model(0, band, MOUNTEMP, target)
            focusenc = self.server.current_position()            
            logging.info('Not moving focus (current position : %d)' % focusenc)
            

        fitsbuilder.append({'FOCOFFST': self.current_offset,
                            'MOUNTEMP': MOUNTEMP,
                            'FOCUSMOD': focus,
                            'FOCUSENC': focusenc,
                            })

    def focus_model(self, x, band, temp, target):
        foc = self.focus20 + (20 - temp) * self.temp_coeff
        foc += self.delta_filter.get(band, 0.)
        foc += self.delta_targets.get(target, 0.)
        return foc + x
    
    def _config(self):
        allinfo=self.server.get_allinfo()
        self.backlash =self.config.Focus.backlash
        self.infstop = self.config.Focus.infstop
        self.nearstop = self.config.Focus.nearstop
        self.position = self.server.current_position()
        # Model parameters
        self.focus20 = self.config.Focus.FOCUS20
        self.delta_filter = self.config.Focus.DELTA_FILTER
        self.delta_targets = self.config.Focus.DELTA_TARGETS
        self.temp_coeff = self.config.Focus.TEMP_COEFF
        self.current_offset = 0
        if not self.server.is_ok():
            raise Exception("focuser not correctly started")

    def _go(self, offset, block=True):
        if offset==0:
            return
        self.server.go(int(offset))
        time.sleep(0.1)
        if block == True:
            while self.server.is_stepping():
                time.sleep(0.1)

    def move(self, position):
        ''' Move focus to given position (absolute)
        '''
        current = self.position
        logging.info('Focuser current position : %d'%current)
        if position > self.infstop or position < self.nearstop:
            logging.error('Focus position outside range: [%d, %d]' % [self.nearstop, self.infstop] )
            return 'Not moving'
        if position < current:
            offset = position - self.backlash - current
            self._go(offset)
            self._go(self.backlash)
        else:
            offset = position - current
            self._go(offset)

        new_position = self.server.current_position()
        if new_position != position:
            raise Exception('expected focus position %d different from actual position %d'%(position, new_position))
        self.position = new_position
        logging.info('New focus position: %d' % position)
        return 'Focus adjusted'

    def adjust(self, band, target, offset):
        ''' Adjust focal plane focus
        '''
        self.current_offset = offset
        MOUNTEMP = float(self.server.get_temperature())
        position = self.focus_model(self.current_offset, band, MOUNTEMP, target)
        return self.move(position)
        
    def plus(self, band, target):
        ''' Adjust focal plane offset toward infinity by 10 steps
        '''
        return self.adjust(band, target, self.current_offset + 10)

    def minus(self, band, target):
        ''' Adjust focal plane offset toward nearby direction by 10 steps
        '''
        return self.adjust(band, target, self.current_offset - 10)
        
    def status(self):
        ret={}
        ret['MOUNTEMP'] = float(self.server.get_temperature())
        self.position = self.server.current_position()
        ret['FOCUSENC'] = self.position
        return ret