<script lang="ts">
    import { environment } from '$lib/runes/env.svelte';
	import type IUrdfJoint from '$lib/components/scene/robot/URDF/interfaces/IUrdfJoint';

    interface Props {}
    
    let {}: Props = $props();
    
    // Helper function to get all robots
    const robots = $derived(Object.entries(environment.robots));
    
    // Helper function to convert degrees to radians
    function degreesToRadians(degrees: number): number {
        return degrees * (Math.PI / 180);
    }
    
    // Helper function to convert radians to degrees
    function radiansToDegrees(radians: number): number {
        return radians * (180 / Math.PI);
    }
    
    // Helper function to get current joint rotation value in degrees
    function getJointRotationValue(joint: any): number {
        const axis = joint.axis_xyz || [0, 0, 1];
        const rotation = joint.rotation || [0, 0, 0];
        
        // Find the primary axis and get the rotation value
        for (let i = 0; i < 3; i++) {
            if (Math.abs(axis[i]) > 0.001) {
                return radiansToDegrees(rotation[i] / axis[i]);
            }
        }
        return 0;
    }
    
    // Helper function to update joint rotation using axis
    function updateJointRotation(robotId: string, jointIndex: number, degrees: number): void {
        const robot = environment.robots[robotId];
        if (!robot?.robot.joints[jointIndex]) return;
        
        const joint = robot.robot.joints[jointIndex];
        const radians = degreesToRadians(degrees);
        const axis = joint.axis_xyz || [0, 0, 1];
        
        // Calculate rotation based on axis
        joint.rotation = [
            radians * axis[0],
            radians * axis[1],
            radians * axis[2]
        ];
    }
    
    // Helper function to get joint limits
    function getJointLimits(joint: IUrdfJoint): { min: number; max: number } {
        if (joint.limit) {
            return {
                min: joint.limit.lower ? radiansToDegrees(joint.limit.lower) : -180,
                max: joint.limit.upper ? radiansToDegrees(joint.limit.upper) : 180
            };
        }
        return joint.type === 'continuous' ? { min: -360, max: 360 } : { min: -180, max: 180 };
    }
</script>

<div class="space-y-6">
    <h3 class="text-lg font-semibold text-slate-100 mb-4">Robot Joint Settings</h3>
    
    {#if robots.length === 0}
        <div class="text-slate-400 text-sm">
            No robots in the environment. Create a robot first using the Control Panel.
        </div>
    {:else}
        {#each robots as [robotId, robotState]}
            <div class="space-y-4 p-4 bg-slate-700/50 rounded-lg border border-slate-600">
                <h4 class="text-md font-medium text-slate-200 border-b border-slate-600 pb-2">
                    Robot: <span class="text-sky-400">{robotId.slice(0, 8)}...</span>
                </h4>
                
                {#if robotState.robot.joints.length === 0}
                    <div class="text-slate-400 text-sm">No joints found for this robot.</div>
                {:else}
                    <div class="text-slate-400 text-sm">
                        Joints:
                    </div>
                    <div class="space-y-4">
                        {#each robotState.robot.joints as joint, jointIndex}
                            {#if joint.type === 'revolute'}
                                {@const currentValue = getJointRotationValue(joint)}
                                {@const limits = getJointLimits(joint)}
                                {@const axis = joint.axis_xyz || [0, 0, 1]}
                                
                                <div class="space-y-3 p-3 bg-slate-800/50 rounded border border-slate-600">
                                    <div class="flex justify-between items-start">
                                        <div>
                                            <h5 class="text-sm font-medium text-slate-300">
                                                <span class="text-yellow-400">{joint.name || `Joint ${jointIndex}`}</span>
                                                <span class="text-xs text-slate-400 ml-2">({joint.type})</span>
                                            </h5>
                                            <div class="text-xs text-slate-500 mt-1">
                                                Axis: [{axis[0].toFixed(1)}, {axis[1].toFixed(1)}, {axis[2].toFixed(1)}]
                                            </div>
                                        </div>
                                        <div class="text-right">
                                            <div class="text-sky-400 font-mono text-sm">{currentValue.toFixed(1)}°</div>
                                            <div class="text-xs text-slate-500">
                                                {limits.min.toFixed(0)}° to {limits.max.toFixed(0)}°
                                            </div>
                                        </div>
                                    </div>
                                    
                                    <!-- Single Rotation Control -->
                                    <div class="space-y-2">
                                        <input 
                                            type="range" 
                                            min={limits.min} 
                                            max={limits.max} 
                                            step="1"
                                            value={currentValue}
                                            oninput={(e) => updateJointRotation(robotId, jointIndex, parseFloat(e.currentTarget.value))}
                                            class="w-full h-2 bg-slate-600 rounded-lg appearance-none cursor-pointer slider"
                                        />
                                    </div>
                                    
                                    <!-- Reset Joint Button -->
                                    <button 
                                        onclick={() => updateJointRotation(robotId, jointIndex, 0)}
                                        class="text-xs px-3 py-1 bg-slate-600 hover:bg-slate-500 text-slate-200 rounded transition-colors"
                                    >
                                        Reset to 0°
                                    </button>
                                </div>
                            {:else if joint.type === 'continuous'}
                                {@const currentValue = getJointRotationValue(joint)}
                                {@const limits = getJointLimits(joint)}
                                {@const axis = joint.axis_xyz || [0, 0, 1]}
                                <div class="space-y-3 p-3 bg-slate-800/50 rounded border border-slate-600">
                                    <div class="flex justify-between items-start">
                                        <div>
                                            <h5 class="text-sm font-medium text-slate-300">
                                                <span class="text-yellow-400">{joint.name || `Joint ${jointIndex}`}</span>
                                                <span class="text-xs text-slate-400 ml-2">({joint.type})</span>
                                            </h5>
                                            <div class="text-xs text-slate-500 mt-1">
                                                Axis: [{axis[0].toFixed(1)}, {axis[1].toFixed(1)}, {axis[2].toFixed(1)}]
                                            </div>
                                        </div>
                                        <div class="text-right">
                                            <div class="text-sky-400 font-mono text-sm">{currentValue.toFixed(1)}°</div>
                                            <div class="text-xs text-slate-500">
                                                {limits.min.toFixed(0)}° to {limits.max.toFixed(0)}°
                                            </div>
                                        </div>
                                    </div>
                                </div>
                            {:else if joint.type === 'fixed'}
                                <div class="p-2 bg-slate-800/30 rounded border border-slate-700">
                                    <div class="text-xs text-slate-500">
                                        <span class="text-slate-400">{joint.name || `Joint ${jointIndex}`}</span>
                                        <span class="ml-2">(fixed joint)</span>
                                    </div>
                                </div>
                            {/if}
                        {/each}
                    </div>
                    {#if robotState.urdfConfig.compoundMovements}
                        <div class="text-slate-400 text-sm">
                            Compound Movements:
                        </div>
                        <div class="space-y-4">
                        {#each robotState.urdfConfig.compoundMovements as movement}
                            <div class="text-slate-400 text-sm">
                                {movement.name}
                                {#each movement.dependents as dependent}
                                    <div class="text-slate-400 text-sm">
                                        {dependent.joint}
                                    </div>
                                {/each}
                            </div>
                        {/each}
                        </div>
                    {/if}
                {/if}
                
                <!-- Reset All Joints Button -->
                <button 
                    onclick={() => {
                        robotState.robot.joints.forEach((joint, index) => {
                            if (joint.type === 'revolute') {
                                updateJointRotation(robotId, index, 0);
                            } else if (joint.type === 'continuous') {
                                updateJointRotation(robotId, index, 0);
                            }
                        });
                    }}
                    class="w-full px-3 py-2 bg-sky-600 hover:bg-sky-500 text-white rounded transition-colors text-sm font-medium"
                >
                    Reset All Joints to 0°
                </button>
            </div>
        {/each}
    {/if}
</div>

<style>
    .slider::-webkit-slider-thumb {
        appearance: none;
        height: 16px;
        width: 16px;
        border-radius: 50%;
        background: #38bdf8; /* sky-400 */
        cursor: pointer;
        box-shadow: 0 1px 3px rgba(0, 0, 0, 0.3);
        border: 2px solid #0f172a; /* slate-900 */
    }
    
    .slider::-moz-range-thumb {
        height: 16px;
        width: 16px;
        border-radius: 50%;
        background: #38bdf8; /* sky-400 */
        cursor: pointer;
        border: 2px solid #0f172a; /* slate-900 */
        box-shadow: 0 1px 3px rgba(0, 0, 0, 0.3);
    }
    
    .slider::-webkit-slider-track {
        background: #475569; /* slate-600 */
        border-radius: 4px;
        height: 8px;
    }
    
    .slider::-moz-range-track {
        background: #475569; /* slate-600 */
        border-radius: 4px;
        height: 8px;
        border: none;
    }
    
    .slider:hover::-webkit-slider-thumb {
        background: #0ea5e9; /* sky-500 */
        transform: scale(1.05);
    }
    
    .slider:hover::-moz-range-thumb {
        background: #0ea5e9; /* sky-500 */
        transform: scale(1.05);
    }
</style>
