#!/bin/bash

#  Copyright (C) 2021 Texas Instruments Incorporated - http://www.ti.com/
#
#  Redistribution and use in source and binary forms, with or without
#  modification, are permitted provided that the following conditions
#  are met:
#
#    Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#
#    Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the
#    distribution.
#
#    Neither the name of Texas Instruments Incorporated nor the names of
#    its contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
#  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
#  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
#  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
#  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
#  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
#  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
#  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
#  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

GREEN='\033[0;32m'
NOCOLOR='\033[0m'

IMX219_CAM_FMT="${IMX219_CAM_FMT:-[fmt:SRGGB8_1X8/1920x1080 field: none]}"

declare -A ALL_UB960_FMT_STR
declare -A ALL_CDNS_FMT_STR
declare -A ALL_CSI2RX_FMT_STR

setup_routes(){
    for i in "${!ALL_UB960_FMT_STR[@]}"
    do
        id="$(cut -d',' -f1 <<<"$i")"
        name="$(cut -d',' -f2 <<<"$i")"
        # UB960 ROUTING & FORMATS
        media-ctl -d $id -R "'$name' [${ALL_UB960_FMT_STR[$i]}]"

        for name in `media-ctl -d $id -p | grep entity | grep imx219 | cut -d ' ' -f 5`; do
            UB953_NAME=`media-ctl -d $id -p -e "imx219 $name" | grep ub953 | cut -d "\"" -f 2`
            UB960_NAME=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d "\"" -f 2`
            UB960_PAD=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d : -f 2 | awk '{print $1}'`
            media-ctl -d $id -V "'$UB960_NAME':$UB960_PAD $IMX219_CAM_FMT"
        done

    done

    # CDNS ROUTING
    for i in "${!ALL_CDNS_FMT_STR[@]}"
    do
        id="$(cut -d',' -f1 <<<"$i")"
        name="$(cut -d',' -f2 <<<"$i")"
        # CDNS ROUTING & FORMATS
        media-ctl -d $id -R "'$name' [${ALL_CDNS_FMT_STR[$i]}]"

        for name in `media-ctl -d $id -p | grep entity | grep imx219 | cut -d ' ' -f 5`; do
            UB953_NAME=`media-ctl -d $id -p -e "imx219 $name" | grep ub953 | cut -d "\"" -f 2`
            UB960_NAME=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d "\"" -f 2`
            UB960_PAD=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d : -f 2 | awk '{print $1}'`
            CSI_PAD=`media-ctl -d $id -p -e "$UB960_NAME" | grep $UB960_PAD/.*[ACTIVE] | cut -d "/" -f 3 | awk '{print $1}'`
            CSI_BRIDGE_NAME=`media-ctl -d $id -p -e "$UB960_NAME" | grep csi-bridge | cut -d "\"" -f 2`
            media-ctl -d $id -V "'$CSI_BRIDGE_NAME':0/$CSI_PAD $IMX219_CAM_FMT"
        done
    done

    # CSI2RX ROUTING
    for i in "${!ALL_CSI2RX_FMT_STR[@]}"
    do
        id="$(cut -d',' -f1 <<<"$i")"
        name="$(cut -d',' -f2 <<<"$i")"
        CSI2RX_NAME=$name
        media-ctl -d $id -R "'$name' [${ALL_CSI2RX_FMT_STR[$i]}]"
        # CSI2RX ROUTING & FORMATS
        for name in `media-ctl -d $id -p | grep entity | grep imx219 | cut -d ' ' -f 5`; do
            UB953_NAME=`media-ctl -d $id -p -e "imx219 $name" | grep ub953 | cut -d "\"" -f 2`
            UB960_NAME=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d "\"" -f 2`
            UB960_PAD=`media-ctl -d $id -p -e "$UB953_NAME" | grep ub960 | cut -d : -f 2 | awk '{print $1}'`
            CSI_PAD=`media-ctl -d $id -p -e "$UB960_NAME" | grep $UB960_PAD/.*[ACTIVE] | cut -d "/" -f 3 | awk '{print $1}'`
            media-ctl -d $id -V "'$CSI2RX_NAME':0/$CSI_PAD $IMX219_CAM_FMT"
        done
    done
}

setup_imx219(){
    count=0
    for media_id in {0..3}; do
    # UB953 FORMATS
    UB960_FMT_STR=""
    CDNS_FMT_STR=""
    CSI2RX_FMT_STR=""
    for name in `media-ctl -d $media_id -p | grep entity | grep imx219 | cut -d ' ' -f 5`; do

        UB953_NAME=`media-ctl -d $media_id -p -e "imx219 $name" | grep ub953 | cut -d "\"" -f 2`
        if [ -n "$UB953_NAME" ]; then

            CAM_SUBDEV=`media-ctl -d $media_id -p -e "imx219 $name" | grep v4l-subdev | awk '{print $4}'`
            media-ctl -d $media_id -V "'imx219 $name':0 $IMX219_CAM_FMT"

            media-ctl -d $media_id -V "'$UB953_NAME':0 $IMX219_CAM_FMT"

            UB960_NAME=`media-ctl -d $media_id -p -e "$UB953_NAME" | grep ub960 | cut -d "\"" -f 2`
            UB960_PAD=`media-ctl -d $media_id -p -e "$UB953_NAME" | grep ub960 | cut -d : -f 2 | awk '{print $1}'`

            CSI_BRIDGE_NAME=`media-ctl -d $media_id -p -e "$UB960_NAME" | grep csi-bridge | cut -d "\"" -f 2`
            CSI2RX_NAME=`media-ctl -d $media_id -p -e "$CSI_BRIDGE_NAME" | grep "ticsi2rx\"" | cut -d "\"" -f 2`

            LAST_PAD=`echo ${ALL_UB960_FMT_STR[$media_id,$UB960_NAME]} | rev | cut -d'/' -f 1 | rev`
            LAST_PAD=${LAST_PAD:0:1}
            if [[ "$LAST_PAD" == "" ]] ; then
                NEXT_PAD=$UB960_PAD
            else
                NEXT_PAD=$(($LAST_PAD+1))
            fi
            CSI2RX_CONTEXT_NAME="$CSI2RX_NAME context $((NEXT_PAD+1))"

            UB960_FMT_STR="${UB960_PAD}/0 -> 4/$(($NEXT_PAD)) [1]"
            CDNS_FMT_STR="0/${NEXT_PAD} -> 1/$(($NEXT_PAD)) [1]"
            CSI2RX_FMT_STR="0/${NEXT_PAD} -> $(($NEXT_PAD+2))/0 [1]"

            # Append UB960 Routes
            if [[ -v "ALL_UB960_FMT_STR[$media_id,$UB960_NAME]" ]] ; then
                ALL_UB960_FMT_STR[$media_id,$UB960_NAME]="${ALL_UB960_FMT_STR[$media_id,$UB960_NAME]}, $UB960_FMT_STR"
            else
                ALL_UB960_FMT_STR[$media_id,$UB960_NAME]="$UB960_FMT_STR"
            fi

            # Append CDNS Routes
            if [[ -v "ALL_CDNS_FMT_STR[$media_id,$CSI_BRIDGE_NAME]" ]] ; then
                ALL_CDNS_FMT_STR[$media_id,$CSI_BRIDGE_NAME]="${ALL_CDNS_FMT_STR[$media_id,$CSI_BRIDGE_NAME]}, $CDNS_FMT_STR"
            else
                ALL_CDNS_FMT_STR[$media_id,$CSI_BRIDGE_NAME]="$CDNS_FMT_STR"
            fi

            # Append CSIRX Routes
            if [[ -v "ALL_CSI2RX_FMT_STR[$media_id,$CSI2RX_NAME]" ]] ; then
                ALL_CSI2RX_FMT_STR[$media_id,$CSI2RX_NAME]="${ALL_CSI2RX_FMT_STR[$media_id,$CSI2RX_NAME]}, $CSI2RX_FMT_STR"
            else
                ALL_CSI2RX_FMT_STR[$media_id,$CSI2RX_NAME]="$CSI2RX_FMT_STR"
            fi

            CAM_DEV=`media-ctl -d $media_id -p -e "$CSI2RX_CONTEXT_NAME" | grep video | awk '{print $4}'`
            CAM_DEV_NAME=/dev/video-imx219-cam-$name

            CAM_SUBDEV_NAME=/dev/v4l-imx219-subdev-$name

            ln -snf $CAM_DEV $CAM_DEV_NAME
            ln -snf $CAM_SUBDEV $CAM_SUBDEV_NAME

            CAM_RESOLUTION=`echo $IMX219_CAM_FMT | cut -d / -f 2 | cut -d " " -f 1`
            CAM_FMT=`echo $IMX219_CAM_FMT | cut -d : -f 2 | cut -d _ -f 1`
            yavta -s $CAM_RESOLUTION -f $CAM_FMT $CAM_DEV &> /dev/null

            echo -e "${GREEN}IMX219 Camera $count detected${NOCOLOR}"
            echo "    device = $CAM_DEV_NAME"
            echo "    name = imx219"
            echo "    format = $IMX219_CAM_FMT"
            echo "    subdev_id = $CAM_SUBDEV_NAME"
            echo "    isp_required = yes"
            echo "    ldc_required = yes"

        else
            IMX219_CAM_FMT='[fmt:SRGGB8_1X8/1920x1080]'
            CAM_SUBDEV=`media-ctl -d $media_id -p -e "imx219 $name" | grep v4l-subdev | awk '{print $4}'`
            media-ctl -d $media_id --set-v4l2 ''"\"imx219 $name\""':0 '$IMX219_CAM_FMT''

            CSI_BRIDGE_NAME=`media-ctl -d $media_id -p -e "imx219 $name" | grep csi-bridge | cut -d "\"" -f 2`
            CSI2RX_NAME=`media-ctl -d $media_id -p -e "$CSI_BRIDGE_NAME" | grep "ticsi2rx\"" | cut -d "\"" -f 2`
            CSI2RX_CONTEXT_NAME="$CSI2RX_NAME context 0"

            media-ctl -d $media_id --set-v4l2 ''"\"$CSI_BRIDGE_NAME\""':0 '$IMX219_CAM_FMT''
            media-ctl -d $media_id --set-v4l2 ''"\"$CSI2RX_NAME\""':0 '$IMX219_CAM_FMT''

            CAM_DEV=`media-ctl -d $media_id -p -e "$CSI2RX_CONTEXT_NAME" | grep video | awk '{print $4}'`
            CAM_DEV_NAME=/dev/video-imx219-cam-$name

            CAM_SUBDEV_NAME=/dev/v4l-imx219-subdev-$name

            ln -snf $CAM_DEV $CAM_DEV_NAME
            ln -snf $CAM_SUBDEV $CAM_SUBDEV_NAME

            echo -e "${GREEN}IMX219 Camera $media_id detected${NOCOLOR}"
            echo "    device = $CAM_DEV_NAME"
            echo "    name = imx219"
            echo "    format = $IMX219_CAM_FMT"
            echo "    subdev_id = $CAM_SUBDEV_NAME"
            echo "    isp_required = yes"
        fi
        count=$(($count + 1))
    done
    done
}

setup_imx219
setup_routes