from ctypes import c_double

import nidaqmx
import numpy as np
from nidaqmx.constants import AcquisitionType, TerminalConfiguration
from nidaqmx.stream_readers import AnalogMultiChannelReader

from toon.input.device import BaseDevice, make_obs


class ForceKeyboard(BaseDevice):
    Forces = make_obs('Forces', (10,), c_double)

    sampling_frequency = 100

    def __init__(self, sampling_frequency=100, **kwargs):
        super(ForceKeyboard, self).__init__(**kwargs)
        self.sampling_frequency = sampling_frequency
        self.period = 1/self.sampling_frequency
        self.t1 = 0
        self._buffer = np.full(self.Forces.shape, np.nan)

    def enter(self):
        # assume first NI DAQ is the one we want
        self._device_name = nidaqmx.system.System.local().devices[0].name
        self._channels = [self._device_name + '/ai' + str(n) for n in
                          [2, 9, 1, 8, 0, 10, 3, 11, 4, 12]]
        self._device = nidaqmx.Task()
        self._device.ai_channels.add_ai_voltage_chan(
            ','.join(self._channels),
            terminal_config=TerminalConfiguration.RSE
        )
        self._reader = AnalogMultiChannelReader(self._device.in_stream)
        self._device.start()

    def read(self):
        self._reader.read_one_sample(self._buffer)
        time = self.clock()
        ret = self.Returns(forces=self.Forces(time, self._buffer))
        while self.clock() - t1 < self.period:
            pass
        self.t1 = self.clock()
        return ret

    def exit(self, *args):
        self._device.stop()
        self._device.close()

from ctypes import c_double

import nidaqmx
import numpy as np
from nidaqmx.constants import AcquisitionType, TerminalConfiguration
from nidaqmx.stream_readers import AnalogMultiChannelReader
from nidaqmx._task_modules.read_functions import _read_analog_f_64
from toon.input.device import BaseDevice, make_obs


class ForceKeyboard(BaseDevice):
    Forces = make_obs('Forces', (2,), c_double)

    def __init__(self, sampling_frequency=250, indices=[7, 8], **kwargs):
        super(ForceKeyboard, self).__init__(**kwargs)
        self.sampling_frequency = sampling_frequency
        self._buffer = np.empty(self.Forces.shape, dtype=c_double)
        if len(indices) > 2:
            raise ValueError('Too many indices for ForceKeyboard.')
        self._indices = indices

    def enter(self):
        # assume first NI DAQ is the one we want
        self._device_name = nidaqmx.system.System.local().devices[0].name
        chans = [2, 9, 1, 8, 0, 10, 3, 11, 4, 12]
        sub_chans = [chans[i] for i in self._indices]
        channels = [self._device_name + ('/ai%i' % n) for n in sub_chans]
        channels = ','.join(channels)
        dev = nidaqmx.Task()
        dev.ai_channels.add_ai_voltage_chan(channels,
                                            terminal_config=TerminalConfiguration.RSE)
        dev.timing.cfg_samp_clk_timing(self.sampling_frequency,
                                       sample_mode=AcquisitionType.CONTINUOUS)
        self._reader = AnalogMultiChannelReader(dev.in_stream)
        dev.start()
        self._device = dev

    def read(self):
        #self._reader.read_one_sample(self._buffer, timeout=0.1)
        try:
            _read_analog_f_64(self._reader._handle, self._buffer, 1, 0)
        except Exception:
            return None
        # TODO: apply calibration?
        time = self.clock()
        return self.Returns(forces=self.Forces(time, self._buffer))

    def exit(self, *args):
        self._device.stop()
        self._device.close()


if __name__ == '__main__':
    import time
    from toon.input import MpDevice
    dev = MpDevice(ForceKeyboard())

    times = []
    with dev:
        start = time.time()
        while time.time() - start < 240:
            dat = dev.read()
            if dat is not None:
                print(dat)
                times.append(dat.time.copy())
            time.sleep(0.016)

    times = np.hstack(times)
    import matplotlib.pyplot as plt

    plt.plot(np.diff(times))
    plt.show()

rewrite force keyboard example