gortty/modulator.go

// Copyright (C) 2017 Keelan Lightfoot
// Copyright (C) 2007-2008 Board of Regents of the University of Wisconsin
//             System (Univ. of Wisconsin-Madison, Trace R&D Center)
// Copyright (C) 2007-2008 Omnitor AB
// Copyright (C) 2007-2008 Voiceriver Inc
//
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.

package gortty

import (
	"context"
	"math"
)

const (
	modStateIdle = iota
	modStateReady
	modStateStart
	modStateBit
	modStateStop
	modStateHold
)

// Modulator is fun
type Modulator struct {
	input  chan byte
	output chan int16

	settings *ModemSettings

	samplesPerBit   int     // number of samples for one bit
	phaseQ16        uint16  // phase acc 0..2math.Pi -> 0..65536
	amplitude       int16   // peak modulator amplitude for one tone
	numStopHalfBits int     // number of stop bits
	numStopSamples  int     // number of samples for stop bit
	wOneQ16         uint16  // scaled one freq 0..2math.Pi -> 0..65536
	wZeroQ16        uint16  // scaled zero freq 0..2math.Pi -> 0..65536
	sinLutQ15       []int16 // sine lookup table
	callback        Callback

	idleDelay int
}

// NewModulator is fun
func NewModulator(ctx context.Context, s *ModemSettings, input chan byte, output chan int16) *Modulator {

	m := new(Modulator)

	m.input = input
	m.output = output

	m.settings = s

	m.samplesPerBit = int(float64(m.settings.SampleRate) / m.settings.Baud)
	m.numStopHalfBits = int(math.Floor(m.settings.StopBits / .5))
	m.numStopSamples = m.numStopHalfBits * m.samplesPerBit / 2
	m.amplitude = 16384
	m.idleDelay = m.settings.SampleRate
	wOne, wZero := calculateW(m.settings.OneFreq, m.settings.ZeroFreq, m.settings.SampleRate)

	m.wOneQ16 = uint16((65536 / (2.0 * math.Pi)) * wOne)
	m.wZeroQ16 = uint16((65536 / (2.0 * math.Pi)) * wZero)

	m.sinLutQ15 = make([]int16, 16384)
	for i := 0; i < 16384; i++ {
		m.sinLutQ15[i] = int16(32767 * math.Sin(2.0*math.Pi*float64(i)/float64(16384)))
	}
	go m.run(ctx)
	return m
}

// SetCallback is fun
func (m *Modulator) SetCallback(callback Callback) {
	m.callback = callback
}

// SetIdleDelay sets how long (in samples) that the modulator must be in the ready state
// before switching to the idle state
func (m *Modulator) SetIdleDelay(d int) {
	m.idleDelay = d
}

func (m *Modulator) modulate(bit int) { //short *buffer, int samples)
	/* use of unsigned short in Q16 for format for phase
	   accumulator leads to natural wrap-around at 2PI
	   boundaries
	*/
	/* sin_lut_q15 is in Q15 format, therefore we rightshift 15 to
	   get Q0 at modulator output.  We cast multiply arguments
	   to 32 bit ints to ensure compiler uses a multiply with a 32
	   bit result.
	*/
	if bit != 0 {
		m.phaseQ16 += m.wOneQ16
		m.output <- multq15(m.amplitude, m.sinLutQ15[m.phaseQ16>>2])

	} else {
		m.phaseQ16 += m.wZeroQ16
		m.output <- multq15(m.amplitude, m.sinLutQ15[m.phaseQ16>>2])
	}
}

func (m *Modulator) SetAmplitude(amplitude int16) {
	m.amplitude = amplitude
}

func (m *Modulator) run(ctx context.Context) {

	var modState int  // modulator state machine
	var nextState int // next state of state machine
	var c byte        // current baudot char being modulated
	var modNbit int   // num bits modulated so far in this char
	var modBit int    // current bit in modBaudot being modulated
	var modSample int // current sample in bit being modulated
	var idleSamples int

	for {
		nextState = modState
		switch modState {
		case modStateReady:
			fallthrough
		case modStateIdle:
			select {
			case <-ctx.Done():
				return
			case c = <-m.input:
				modSample = 0
				nextState = modStateStart
				idleSamples = 0
				if m.callback != nil {
					m.callback(EventStartBit, 0)
					if modState == modStateIdle {
						m.callback(EventActive, 0)
					}
				}
			default:
				m.modulate(1)

				if modState == modStateReady {
					if idleSamples < m.idleDelay {
						idleSamples++
					} else {
						nextState = modStateIdle
						if m.callback != nil {
							m.callback(EventIdle, 0)
						}
						idleSamples++
					}
				}
			}
		case modStateStart:
			m.modulate(0)
			modSample++
			// after sending start bit send LSB data bit
			if modSample == m.samplesPerBit {
				modNbit = 0
				modBit = int(c & 0x1)
				modSample = 0
				nextState = modStateBit

			}
		case modStateBit:
			m.modulate(modBit)
			modSample++
			// when this data bit complete, send next data bit
			if modSample == m.samplesPerBit {
				modSample = 0
				modNbit++
				modBit = int((c >> uint(modNbit)) & 0x1)
				if modNbit == m.settings.DataBits {
					nextState = modStateStop
					//	fmt.Println("modStateStop")

				}
			}
		case modStateStop:
			m.modulate(1)
			modSample++
			// when stop bit complete start transmission of next char, or enter idle state
			if modSample == m.numStopSamples {
				modSample = 0
				nextState = modStateReady
				if m.callback != nil {
					m.callback(EventStopBit, 0)
				}
			}
			// 	(obl.callback)(obl,OBL_EVENT_TX_STATE, OBL_TRANSMIT_STOP);
		}
		modState = nextState
	}

}