package me.drton.jmavsim.vehicle;

import me.drton.jmavsim.Rotor;
import me.drton.jmavsim.World;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;
import java.io.FileNotFoundException;

/**
 * Generic hexacopter model.
 */
public class Hexacopter extends AbstractMulticopter {
    private static final int rotorsNum = 6;
    private Vector3d[] rotorPositions = new Vector3d[rotorsNum];

    /**
     * Generic hexacopter constructor.
     *
     * @param world          world where to place the vehicle
     * @param modelName      filename of model to load, in .obj format
     * @param orientation    "x" or "+"
     * @param armLength      length of arm from center
     * @param rotorThrust    full thrust of one rotor
     * @param rotorTorque    torque at full thrust of one rotor
     * @param rotorTimeConst spin-up time of rotor
     * @param rotorsOffset   rotors positions offset from gravity center
     * @throws FileNotFoundException if .obj model file not found
     */
    public Hexacopter(World world, String modelName, String orientation, double armLength, double rotorThrust,
                      double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException {
        super(world, modelName);
        rotorPositions[0] = new Vector3d(armLength, 0.0, 0.0);
        rotorPositions[1] = new Vector3d(-armLength, 0.0, 0.0);
        rotorPositions[2] = new Vector3d(-armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
        rotorPositions[3] = new Vector3d(armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
        rotorPositions[4] = new Vector3d(armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
        rotorPositions[5] = new Vector3d(-armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
        if (orientation.equals("x") || orientation.equals("X")) {
            Matrix3d r = new Matrix3d();
            r.rotZ(Math.PI / 2);
            for (int i = 0; i < rotorsNum; i++) {
                r.transform(rotorPositions[i]);
            }
        } else if (orientation.equals("+")) {
        } else {
            throw new RuntimeException("Unknown orientation: " + orientation);
        }
        for (int i = 0; i < rotors.length; i++) {
            rotorPositions[i].add(rotorsOffset);
            Rotor rotor = rotors[i];
            rotor.setFullThrust(rotorThrust);
            rotor.setFullTorque((i == 1 || i == 3 || i == 4) ? -rotorTorque : rotorTorque);
            rotor.setTimeConstant(rotorTimeConst);
        }
    }

    @Override
    protected int getRotorsNum() {
        return rotorsNum;
    }

    @Override
    protected Vector3d getRotorPosition(int i) {
        return rotorPositions[i];
    }
}