package Camera;

import com.fazecast.jSerialComm.SerialPort;
import id.co.gtc.Main;
import org.tinylog.Logger;


/**
 * Pan Tilt Controller
 * Using PelcoD protocol
 * Source : <a href="https://www.commfront.com/pages/pelco-d-protocol-tutorial">PelcoD Tutorial</a>
 */
public class PanTiltController {
    private SerialPort serialPort;
    private final byte cameraid;
    /**
     * Open Pan Tilt Controller
     * @param portname serial port name used
     * @param baudrate baudrate used
     */
    public PanTiltController(String portname, int baudrate, int cameraid){
        this.cameraid = (byte)cameraid;
        SerialPort[] comports = SerialPort.getCommPorts();
        if (comports.length>0){
            for (SerialPort port : comports){
                Logger.info("Available Serial Port : {}", port.getSystemPortName());
                if (port.getSystemPortName().equals(portname)){
                    Logger.info("Serial Port {} found", portname);
                    serialPort = port;

                    break;
                }
            }
            if (serialPort!=null){
                serialPort.setBaudRate(baudrate);
                if (serialPort.openPort()){
                    Logger.info("Serial Port {} opened successfully at {}", portname, baudrate);
                } else {
                    Logger.info("Failed to open Serial Port {} at {}", portname, baudrate);
                }
            } else Logger.info("Serial Port {} not found", portname);
        } else Logger.info("No Serial Port found");

    }

    /**
     * Close Pan Tilt Controller
     */
    public void Close(){
        if (serialPort!=null) serialPort.closePort();
        Logger.info("Serial Port closed");
    }

    /**
     * Check if Serial Port is open
     * @return true if open, false otherwise
     */
    public boolean isOpen(){
        return serialPort != null && serialPort.isOpen();
    }

    /**
     * Stop Pan Tilt Movement
     */
    public void StopMovement(){
        byte[] command = new byte[]{0, cameraid, 0, 0, 0, 0, 0};
        command[6] = Checksum(command);     // add checksum
        command[0] = (byte) 0xFF;           // add synchronization byte
        if (isOpen()) {
            Main.Max485Direction(true);
            serialPort.writeBytes(command, command.length);
            Main.Max485Direction(false);
            Main.Blink_LedPanTilt();
        }
    }

    /**
     * Pan Left
     * @param speed speed of movement, 0-63
     */
    public void PanLeft(byte speed){
        if (speed<0) speed = 0;
        if (speed>0x3F) speed = 0x3F;

        byte[] command = new byte[]{0, cameraid, 0, 4, speed, 0, 0};
        command[6] = Checksum(command);     // add checksum
        command[0] = (byte) 0xFF;           // add synchronization byte
        if (isOpen()) {
            Main.Max485Direction(true);
            serialPort.writeBytes(command, command.length);
            Main.Max485Direction(false);
            Main.Blink_LedPanTilt();
        }
    }

    /**
     * Pan Right
     * @param speed speed of movement, 0-63
     */
    public void PanRight(byte speed){
        if (speed<0) speed = 0;
        if (speed>0x3F) speed = 0x3F;

        byte[] command = new byte[]{0, cameraid, 0, 2, speed, 0, 0};
        command[6] = Checksum(command);     // add checksum
        command[0] = (byte) 0xFF;           // add synchronization byte
        if (isOpen()) {
            Main.Max485Direction(true);
            serialPort.writeBytes(command, command.length);
            Main.Max485Direction(false);
            Main.Blink_LedPanTilt();
        }
    }

    /**
     * Tilt Up
     * @param speed speed of movement, 0-63
     */
    public void TiltUp(byte speed){
        if (speed<0) speed = 0;
        if (speed>0x3F) speed = 0x3F;

        byte[] command = new byte[]{0, cameraid, 0, 8, speed, 0, 0};
        command[6] = Checksum(command);     // add checksum
        command[0] = (byte) 0xFF;           // add synchronization byte
        if (isOpen()) {
            Main.Max485Direction(true);
            serialPort.writeBytes(command, command.length);
            Main.Max485Direction(false);
            Main.Blink_LedPanTilt();
        }
    }

    /**
     * Tilt Down
     * @param speed speed of movement, 0-63
     */
    public void TiltDown(byte speed){
        if (speed<0) speed = 0;
        if (speed>0x3F) speed = 0x3F;

        byte[] command = new byte[]{0, cameraid, 0, 16, speed, 0, 0};
        command[6] = Checksum(command);     // add checksum
        command[0] = (byte) 0xFF;           // add synchronization byte
        if (isOpen()) {
            Main.Max485Direction(true);
            serialPort.writeBytes(command, command.length);
            Main.Max485Direction(false);
            Main.Blink_LedPanTilt();
        }
    }

    /**
     * Sum of bytes, then modulo by 256
     * @param data data to be summed
     * @return checksum
     */
    private byte Checksum(byte[] data){
        int sum = 0;
        for (byte b : data){
            sum += b;
        }

        return (byte)(sum % 256);
    }
}
