private void posUpdater(CancellationToken aCancellationToken)
{
LinearCurrentTime = DateTime.Now.Ticks;
while (!aCancellationToken.IsCancellationRequested)
{
var now = DateTime.Now.Ticks;
if (LinearCurrentPosition != LinearTargetPosition)
{
var diff = FleshlightHelper.GetDistance(Convert.ToUInt32(new TimeSpan(now - LinearCurrentTime).TotalMilliseconds), LinearSpeed);
var diff2 = LinearTargetPosition - LinearCurrentPosition;
if (diff2 < 0)
{
diff *= -1;
diff = Math.Max(diff2, diff);
}
else
{
diff = Math.Min(diff2, diff);
}
LinearCurrentPosition += diff;
Dispatcher.Invoke(() =>
{
LinearPosition.Value = LinearCurrentPosition * LinearPosition.Maximum;
});
}
LinearCurrentTime = now;
Thread.Sleep(10);
}
}
private Task <ButtplugMessage> HandleFleshlightLaunchCmd(ButtplugDeviceMessage aMsg)
{
if (!(aMsg is FleshlightLaunchFW12Cmd cmdMsg))
{
return(Task.FromResult <ButtplugMessage>(BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler")));
}
lock (_movementLock)
{
_speed = (Convert.ToDouble(cmdMsg.Speed) / 99) * 100;
_position = (Convert.ToDouble(cmdMsg.Position) / 99) * 100;
_position = _position < 0 ? 0 : _position;
_position = _position > 100 ? 100 : _position;
_speed = _speed < 20 ? 20 : _speed;
_speed = _speed > 100 ? 100 : _speed;
// This is @funjack's algorithm for converting Fleshlight Launch
// commands into absolute distance (percent) / duration (millisecond) values
var distance = Math.Abs(_position - _currentPosition);
var duration = FleshlightHelper.GetDuration(distance / 100, _speed / 100);
// We convert those into "position" increments for our OnUpdate() timer event.
_increment = 1.5 * (distance / (duration / _updateInterval));
return(Task.FromResult <ButtplugMessage>(new Ok(aMsg.Id)));
}
}
private Task <ButtplugMessage> HandleFleshlightLaunchFW12Cmd([NotNull] ButtplugDeviceMessage aMsg, CancellationToken aToken)
{
var cmdMsg = CheckMessageHandler <FleshlightLaunchFW12Cmd>(aMsg);
var pos = Convert.ToDouble(cmdMsg.Position) / 99.0;
var dur = Convert.ToUInt32(FleshlightHelper.GetDuration(Math.Abs((1 - pos) - _currentPosition), cmdMsg.Speed / 99.0));
return(HandleLinearCmd(LinearCmd.Create(cmdMsg.DeviceIndex, cmdMsg.Id, dur, pos, 1), aToken));
}
public void TestOutOfBounds()
{
(Math.Abs(FleshlightHelper.GetSpeed(0, 500) - FleshlightHelper.GetSpeed(-1, 500)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetSpeed(1, 500) - FleshlightHelper.GetSpeed(2, 500)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDistance(100, 0) - FleshlightHelper.GetDistance(100, -1)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDistance(100, 1) - FleshlightHelper.GetDistance(100, 2)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDistance(1000, 0.17379819904439015016403395523936)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDuration(0.5, 0) - FleshlightHelper.GetDuration(0.5, -1)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDuration(0.5, 1) - FleshlightHelper.GetDuration(0.5, 2)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDuration(0, 0.5) - FleshlightHelper.GetDuration(-1, 0.5)) < 0.0001).Should().BeTrue();
(Math.Abs(FleshlightHelper.GetDuration(1, 0.5) - FleshlightHelper.GetDuration(2, 0.5)) < 0.0001).Should().BeTrue();
}
private Task <ButtplugMessage> HandleFleshlightLaunchCmd(ButtplugDeviceMessage aMsg, CancellationToken aToken)
{
var cmdMsg = CheckMessageHandler <FleshlightLaunchFW12Cmd>(aMsg);
_speed = (Convert.ToDouble(cmdMsg.Speed) / 99) * 100;
_position = (Convert.ToDouble(cmdMsg.Position) / 99) * 100;
_position = _position < 0 ? 0 : _position;
_position = _position > 100 ? 100 : _position;
_speed = _speed < 20 ? 20 : _speed;
_speed = _speed > 100 ? 100 : _speed;
// This is @funjack's algorithm for converting Fleshlight Launch commands into absolute
// distance (percent) / duration (millisecond) values
var distance = Math.Abs(_position - _currentPosition);
var duration = FleshlightHelper.GetDuration(distance / 100, _speed / 100);
// We convert those into "position" increments for our OnUpdate() timer event.
_increment = 1.5 * (distance / (duration / _updateInterval));
return(Task.FromResult <ButtplugMessage>(new Ok(aMsg.Id)));
}
public void TestRoundTrip()
{
var delta = FleshlightHelper.GetDistance(100, 0.5);
var speed = FleshlightHelper.GetSpeed(delta, 100);
var time = FleshlightHelper.GetDuration(delta, 0.5);
(Math.Abs(0.5 - speed) < 0.01).Should().BeTrue();
(Math.Abs(100 - time) < 0.01).Should().BeTrue();
var speed2 = FleshlightHelper.GetSpeed(0.5, 500);
var delta2 = FleshlightHelper.GetDistance(500, speed2);
var time2 = FleshlightHelper.GetDuration(0.5, speed2);
(Math.Abs(0.5 - delta2) < 0.01).Should().BeTrue();
(Math.Abs(500 - time2) < 10).Should().BeTrue();
var time3 = Convert.ToUInt32(FleshlightHelper.GetDuration(0.5, 0.5));
var speed3 = FleshlightHelper.GetSpeed(0.5, time3);
var delta3 = FleshlightHelper.GetDistance(time3, 0.5);
(Math.Abs(0.5 - delta3) < 0.01).Should().BeTrue();
(Math.Abs(0.5 - speed3) < 0.01).Should().BeTrue();
}
private async Task <ButtplugMessage> HandleLinearCmd(ButtplugDeviceMessage aMsg)
{
var cmdMsg = aMsg as LinearCmd;
if (cmdMsg is null)
{
return(BpLogger.LogErrorMsg(aMsg.Id, Error.ErrorClass.ERROR_DEVICE, "Wrong Handler"));
}
if (cmdMsg.Vectors.Count != 1)
{
return(new Error(
"LinearCmd requires 1 vector for this device.",
Error.ErrorClass.ERROR_DEVICE,
cmdMsg.Id));
}
foreach (var v in cmdMsg.Vectors)
{
if (v.Index != 0)
{
return(new Error(
$"Index {v.Index} is out of bounds for LinearCmd for this device.",
Error.ErrorClass.ERROR_DEVICE,
cmdMsg.Id));
}
return(await HandleFleshlightLaunchRawCmd(new FleshlightLaunchFW12Cmd(cmdMsg.DeviceIndex,
Convert.ToUInt32(FleshlightHelper.GetSpeed(Math.Abs(_lastPosition - v.Position), v.Duration) * 99),
Convert.ToUInt32(v.Position * 99), cmdMsg.Id)));
}
return(new Ok(aMsg.Id));
}
private void pipeReader(CancellationToken aCancellationToken)
{
while (!aCancellationToken.IsCancellationRequested && _pipeClient != null && _pipeClient.IsConnected)
{
var buffer = new byte[4096];
string msg = string.Empty;
var len = -1;
while (len < 0 || (len == buffer.Length && buffer[4095] != '\0'))
{
try
{
var waiter = _pipeClient.ReadAsync(buffer, 0, buffer.Length);
while (!waiter.GetAwaiter().IsCompleted)
{
if (!_pipeClient.IsConnected)
{
return;
}
Thread.Sleep(10);
}
len = waiter.GetAwaiter().GetResult();
}
catch
{
continue;
}
if (len > 0)
{
msg += Encoding.ASCII.GetString(buffer, 0, len);
}
}
Console.Out.WriteLine(msg);
var parsed = _parser.Deserialize(msg);
if (parsed == null)
{
continue;
}
switch (parsed)
{
case StartScanning s1:
foreach (DeviceSimulator dev in tabs.Values)
{
var devAdded = new DeviceAdded();
devAdded.Name = dev.Name;
devAdded.Id = dev.Id;
devAdded.HasLinear = dev.HasLinear;
devAdded.VibratorCount = dev.VibratorCount;
devAdded.HasRotator = dev.HasRotator;
_msgQueue.Enqueue(devAdded);
}
_msgQueue.Enqueue(new FinishedScanning());
break;
case StopScanning s1:
break;
case StopDevice sd:
foreach (DeviceSimulator dev in tabs.Values)
{
if (dev == null || dev.Id != sd.Id)
{
return;
}
dev.StopDevice();
}
break;
case Vibrate v:
foreach (DeviceSimulator dev in tabs.Values)
{
if (dev == null || dev.Id != v.Id)
{
return;
}
dev.Vibrate(v.VibratorId, v.Speed);
}
break;
case Linear l:
foreach (DeviceSimulator dev in tabs.Values)
{
if (dev == null || dev.Id != l.Id)
{
return;
}
if (dev.HasLinear)
{
dev.LinearTargetPosition = l.Position;
dev.LinearSpeed = l.Speed;
}
}
break;
case Linear2 l:
foreach (DeviceSimulator dev in tabs.Values)
{
if (dev == null || dev.Id != l.Id)
{
return;
}
if (dev.HasLinear)
{
double dist = Math.Abs(dev.LinearTargetPosition - l.Position);
dev.LinearSpeed = FleshlightHelper.GetSpeed(dist, l.Duration);
dev.LinearTargetPosition = l.Position;
}
}
break;
case Rotate r:
foreach (DeviceSimulator dev in tabs.Values)
{
if (dev == null || dev.Id != r.Id)
{
return;
}
if (dev.HasRotator)
{
Dispatcher.Invoke(() =>
{
dev.RotatorSpeed.Value = (Convert.ToDouble(Math.Min(r.Speed, 99)) / 100) * dev.RotatorSpeed.Maximum;
dev.RotatorSpeed.Foreground = r.Clockwise ? Brushes.Red : Brushes.GreenYellow;
});
}
}
break;
case Ping p:
_msgQueue.Enqueue(new Ping());
break;
default:
break;
}
}
if (_pipeClient != null && _pipeClient.IsConnected)
{
_pipeClient.Close();
}
_pipeClient = null;
Dispatcher.Invoke(() =>
{
Connect.Content = "Connect";
});
}
private async Task <ButtplugMessage> HandleLinearCmd(ButtplugDeviceMessage aMsg, CancellationToken aToken)
{
var cmdMsg = CheckMessageHandler <LinearCmd>(aMsg);
if (cmdMsg.Vectors.Count != 1)
{
throw new ButtplugDeviceException(BpLogger,
"LinearCmd requires 1 vector for this device.",
cmdMsg.Id);
}
foreach (var v in cmdMsg.Vectors)
{
if (v.Index != 0)
{
throw new ButtplugDeviceException(BpLogger,
$"Index {v.Index} is out of bounds for LinearCmd for this device.",
cmdMsg.Id);
}
return(await HandleFleshlightLaunchCmd(new FleshlightLaunchFW12Cmd(cmdMsg.DeviceIndex,
Convert.ToUInt32(FleshlightHelper.GetSpeed(Math.Abs((_position / 100) - v.Position), v.Duration) * 99),
Convert.ToUInt32(v.Position * 99), cmdMsg.Id), aToken).ConfigureAwait(false));
}
return(new Ok(aMsg.Id));
}
private async Task <ButtplugMessage> HandleLinearCmd(ButtplugDeviceMessage aMsg, CancellationToken aToken)
{
var cmdMsg = CheckGenericMessageHandler <LinearCmd>(aMsg, 1);
var v = cmdMsg.Vectors[0];
return(await HandleFleshlightLaunchFW12Cmd(new FleshlightLaunchFW12Cmd(cmdMsg.DeviceIndex,
Convert.ToUInt32(FleshlightHelper.GetSpeed(Math.Abs(_lastPosition - v.Position), v.Duration) * 99),
Convert.ToUInt32(v.Position * 99), cmdMsg.Id), aToken).ConfigureAwait(false));
}
