001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.common.util;
019
020import edu.wpi.first.math.geometry.Transform3d;
021import java.util.HashMap;
022import org.photonvision.common.logging.LogGroup;
023import org.photonvision.common.logging.Logger;
024
025public final class SerializationUtils {
026    private static final Logger logger = new Logger(SerializationUtils.class, LogGroup.General);
027
028    public static HashMap<String, Object> objectToHashMap(Object src) {
029        var ret = new HashMap<String, Object>();
030        for (var field : src.getClass().getFields()) {
031            try {
032                field.setAccessible(true);
033                if (!field
034                        .getType()
035                        .isEnum()) { // if the field is not an enum, get it based on the current pipeline
036                    ret.put(field.getName(), field.get(src));
037                } else {
038                    var ordinal = (Enum) field.get(src);
039                    ret.put(field.getName(), ordinal.ordinal());
040                }
041            } catch (IllegalArgumentException | IllegalAccessException e) {
042                logger.error("Could not serialize " + src.getClass().getSimpleName(), e);
043            }
044        }
045        return ret;
046    }
047
048    public static HashMap<String, Object> transformToHashMap(Transform3d transform) {
049        var ret = new HashMap<String, Object>();
050        ret.put("x", transform.getTranslation().getX());
051        ret.put("y", transform.getTranslation().getY());
052        ret.put("z", transform.getTranslation().getZ());
053        ret.put("qw", transform.getRotation().getQuaternion().getW());
054        ret.put("qx", transform.getRotation().getQuaternion().getX());
055        ret.put("qy", transform.getRotation().getQuaternion().getY());
056        ret.put("qz", transform.getRotation().getQuaternion().getZ());
057
058        ret.put("angle_x", transform.getRotation().getX());
059        ret.put("angle_y", transform.getRotation().getY());
060        ret.put("angle_z", transform.getRotation().getZ());
061        return ret;
062    }
063}