001/* Copyright (C) Cross The Road Electronics 2024 */
002package com.ctre.phoenix.motion;
003
004import com.ctre.phoenix.ErrorCode;
005import com.ctre.phoenix.motion.TrajectoryPoint;
006import com.ctre.phoenix.motion.BuffTrajPointStreamJNI;
007
008/**
009 * Stream of trajectory points for Talon/Victor motion profiling.
010 */
011public class BufferedTrajectoryPointStream{
012    private long m_handle;
013
014    public BufferedTrajectoryPointStream()
015    {
016        m_handle = BuffTrajPointStreamJNI.Create1();
017    }
018        /**
019         * Clear all trajectory points.
020         * @return nonzero error code if operation fails.
021         */
022    public ErrorCode Clear()
023    {
024        return ErrorCode.valueOf(BuffTrajPointStreamJNI.Clear(m_handle));
025    }
026        /**
027         * Write a single trajectory point into the buffer.
028         * @param trajPt        Trajectory point to write.
029         * @return nonzero error code if operation fails.
030         */
031    public ErrorCode Write(TrajectoryPoint trajPt)
032    {
033        int status = BuffTrajPointStreamJNI.Write(m_handle,
034            trajPt.position,
035            trajPt.velocity,
036            trajPt.arbFeedFwd,
037            trajPt.auxiliaryPos,
038            trajPt.auxiliaryVel,
039            trajPt.auxiliaryArbFeedFwd,
040            trajPt.profileSlotSelect0,
041            trajPt.profileSlotSelect1,
042            trajPt.isLastPoint,
043            trajPt.zeroPos,
044            trajPt.timeDur,
045            trajPt.useAuxPID);
046        return ErrorCode.valueOf(status);
047    }
048        /**
049         * Writes an array of trajectory point into the buffer.
050         * @param trajPts       Array of trajectory points to write.
051         * @param trajPtCount  Number of points to write.  This is capped against array length.
052         * @return nonzero error code if operation fails.
053         */
054    public ErrorCode Write(TrajectoryPoint[] trajPts, int trajPtCount)
055    {
056        ErrorCode retval = ErrorCode.OK;
057
058        if(trajPtCount > trajPts.length){trajPtCount = trajPts.length;}
059
060        for (int i = 0; i < trajPtCount; ++i) {
061            /* insert next pt */
062            ErrorCode er = Write(trajPts[i]);
063            /* save first nonzero error code */
064            if (retval == ErrorCode.OK) { retval = er; }
065        }
066
067        return retval;
068    }
069        /**
070         * Writes an array of trajectory point into the buffer.
071         * @param trajPts       Array of trajectory points to write.
072         * @return nonzero error code if operation fails.
073         */
074    public ErrorCode Write(TrajectoryPoint[] trajPts){
075        return Write(trajPts, trajPts.length);
076    }
077
078    /**
079     * @return handle of object
080     */
081    public long getHandle(){return m_handle;}
082}