/* Copyright (c) 1998 Regents of the University of California.
 * rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgment:
 *       This product includes software developed by the Imaging and
 *       Distributed Collaboration Group at Lawrence Berkeley 
 *       National Laboratory.
 * 4. Neither the name of the University nor of the Laboratory may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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.
 *
 * @(#) $Header: canonvcc1.cpp,v 0.4.2 98/08/10 10:28:31 mperry Exp $ (LBL)
*/


#include "canonvcc1.h"

CanonVCC1::CanonVCC1(Serialport *port, const char *p, int n, short d, short l) 
	: Camera(n, d, l)	
{
	int x;

	if (debugLevel > 0)
	   fprintf(stderr,"INITIALIZING THE CANON VC-C1\n");
	initialized = 0;
	avail = 0;
	setPort(port);
	if (devnum == 1)
	    portp->openPort(p);
	if (portp->getFileDesc() == (HANDLE)-1)
	{			
		if (debugLevel > 0)
			fprintf(stderr,"CAN'T OPEN A PORT FOR THE CANON VC-C1\n");
	}
	if (devnum == 1)
	{
		x = portp->setupPort((DWORD)9600,(BYTE)8,(BYTE)0,(BYTE)TWOSTOPBITS);
	    if (x < 0)
	    {
			if (debugLevel > 0)
				fprintf(stderr,"CAN'T SET UP A PORT FOR THE CANON VC-C1\n");
			return;
	    }	
	}
	x = init();
	if (!avail)
	{
	    if (debugLevel > 0)
	    {
	       fprintf(stderr,"INITIALIZATION FAILED FOR THE CANON VC-C1\n");
	       fprintf(stderr,"CHECK THE CONNECTION/POWER\n");
	    }
	    return;
	}	
	initialized = 1;
	if (devnum == 1)
	{
	    // Re-initialize all static variables (values apply to every
	    // canon vcc1 camera). 
	    minpan = -50.0;
	    maxpan = 50.0;
	    mintilt = -20.0;
	    maxtilt = 20.0; 
	    min_pspeed = 100.0;
	    max_pspeed = 500.0;
	    min_tspeed = 100.0;
	    max_tspeed = 500.0;
	    min_zspeed = 100.0;
	    max_zspeed = 500.0;
	    zoom_limit = 128.0;
	    minzoom = 1.0;
	    maxzoom = 12.0;
	}
	if (!lens)
	{	
	    pcoeff = (float)64.586;
	    pexp = (float)-0.198;
	    tcoeff = (float)41.43;
	    texp = (float)-0.182;
	    zcoeffw = (float)12.244;
	    zexpw = (float)-0.223; 
	    zcoeffh = (float)8.891;
	    zexph = (float)-0.224;	
	}
	else
	{
	    pcoeff = (float)65.3;
	    pexp = (float)-0.154;
	    tcoeff = (float)40.447;
	    texp = (float)-0.118;
	    zcoeffw = (float)13.423;
	    zexpw = (float)-0.204; 
	    zcoeffh = (float)13.985;
	    zexph = (float)-0.243;	
	}
	pspeed = max_pspeed;
	tspeed = max_tspeed;
	zspeed = max_zspeed;
	pspeed = max_pspeed;
	tspeed = max_tspeed;
}

void CanonVCC1::setPort(Serialport *p)
{
	portp = p;
	assert(portp != NULL);
} 


int CanonVCC1::init()
{
	int x;
	unsigned char buf[15];

	// Turn on the power.
	x = doPower();
	if (x) 
	{
	    // Send the Pedestal Initialize 1 cmd.
	
	    buf[0] = 0x00;
	    buf[1] = 0x58;
	    buf[2] = 0x30; 
	    x = transmit(buf, 3);
	}
	return x;
}

int CanonVCC1::getOperationalStatus(unsigned char reply[])
{
	unsigned char buf[20];
	int x;

	buf[0] = 0x00;
	buf[1] = 0x86;
	x = transmit(buf, 2);
	if (debugLevel > 2)
	{
	   printf("CanonVCC1::getOperationalStatus--transmit returned %d\n",x);
	   printf("getOperationalStatus: transmit returned reply: ");
	   displayString("getOpStatus reply", buf, x); 	
	}
	for (int i=0; i<x; i++)
	    reply[i] = buf[i]; 	
	return x;
}


int CanonVCC1::move(char *cmd)
{
    struct Cmdline cmds[6];  // store multiple commands
    int i, n=0, j, pt=0, pspd=0, tspd=0, zspd=0;

    if (!initialized)
		return -1;
    i = parseCmd(cmd, cmds, &pt, &pspd, &tspd, &zspd); 	
    if (pspd)
        n = doPtSpeedCmd('p');
    if (tspd)
        n = doPtSpeedCmd('t');

    // Walk array and carry out requested commands in order of occurrence.
    for (j=0; j<i; j++)
    {
        if (!strcmp(cmds[j].axis,"pan"))
			n = doPan(); 
        else if((cmds[j].axis[0]=='t')&&(cmds[j].axis[1]=='i')&&(cmds[j].axis[2]=='l'))
			n = doTilt(); 
		else if ((cmds[j].axis[0]=='p')&&(cmds[j].axis[1]=='o')&&(cmds[j].axis[2]=='w'))
		{
			n = stringToInt(cmds[j].value);
			if (power != n)
			{
				power = n;
				n = doPower();
			}	
		}
		else if (!strcmp(cmds[j].axis, "shutdown") && avail)
			n = shutDown();
		else if (!strcmp(cmds[j].axis, "home"))
			n = goHome();
		else if (!strcmp(cmds[j].axis, "zoom"))
			n = doZoom();
    } 
    if ((n < 0) && (debugLevel > 0))
    {	
      fprintf(stderr,"CANON VC-C1 %d NOT RESPONDING...CHECK CONNECTION/POWER\n",
	   devnum); 
    }	
    return n;
}

void CanonVCC1::waitNotBusy()
{
    /* If the Canon VC-C1 is moving (panning, tilting, zooming, or focusing,
       it will ignore commands.  So send the operational status request and 
	   check values of status bytes (per page 12 of the Canon VC-C1 API.  
	   Wait until not busy or 3 seconds have elapsed.  */

    int x, n=0;
    unsigned char reply[15];

    x = getOperationalStatus(reply); 
    while ((n<30)&&((reply[5]==0x32)||(reply[5]==0x33)||(reply[5]>0x35)||(reply[6]==0x38)||(reply[7]==0x31)))
    {
		Sleep(3000);
		x = getOperationalStatus(reply);
		n++;
    }
}


int CanonVCC1::doPtSpeedCmd(char c)
{
    int x;
    unsigned char cmd[20];

	cmd[0] = 0x00;
	if (c == 'p')
	{
	    cmd[1] = 0x50;
       	x = (int)pspeed;
	}
	else if (c == 't')
	{
	    cmd[1] = 0x51;
        x = (int)tspeed;
	}
    intToString(x, 3, (char *)&cmd[2]);
    x = transmit(cmd, 5);
    return x;
}

int CanonVCC1::doPan()
{
    unsigned char req[20];
    int x;

	req[0] = '\x00';
	req[1] = '\x54';
    x = convert("pan", maxpan);
    intToString(x, 3, (char *)&req[2]);
	waitNotBusy();
    x = transmit(req, 5);
    return x;
}


int CanonVCC1::doTilt()
{
    unsigned char req[20];
    int x;
	req[0] = '\x00';
	req[1] = '\x55';
    x = convert("tilt", maxtilt);
    intToString(x, 3, (char *)&req[2]);
	waitNotBusy();
    x = transmit(req, 5);
    return x;
}

int CanonVCC1::doZoom()
{
    unsigned char req[20];
    int x;

	req[0] = 0x00;
	req[1] = 0xA3;
    x = convert("zoom", zoom_limit);
	intToString(x,2,(char *)&req[2]);
	waitNotBusy();
	if (debugLevel > 3)
	   displayString("CanonVCC1--transmitting ZOOM CMD", req, 4); 
	x = transmit(req, 4);
    return x;
}


int CanonVCC1::goHome()
{
	int x;
	unsigned char buf[20];

	if (debugLevel > 2)
	    printf("CanonVCC1::goHome--sending P/T HOME cmd\n");
	// Move the Pan/Tilter to home position.
	buf[0] = 0x00;
	buf[1] = 0x57;
	waitNotBusy();
	x = transmit(buf, 2);
	// Zoom to home zoom position.
	zposn = homezoom;
	if (debugLevel > 2)
	    printf("CanonVCC1::goHome--calling doZoom\n");
	waitNotBusy();
	doZoom();
	return x;
}


int CanonVCC1::shutDown()
{
	int x;
	power = 0;
	x = doPower(); 
	return x;	
}

int CanonVCC1::doPower()
{
	int x; 
	unsigned char buf[20];
	buf[0] = '\x00';
	buf[1] = '\xA0';
	if (power)
	    buf[2] = '\x31';
	else
	    buf[2] = '\x30'; 	
	x = transmit(buf, 3);
	return x;
}


int CanonVCC1::transmit(unsigned char *buf, int len)
{
	int i,n;
	unsigned char cmd[20]; 

	cmd[0] = 0xFF;
	cmd[1] = 0x30;
	cmd[2] = 0x30;
	for (n=0,i=3; n<len; n++,i++)
		cmd[i] = buf[n];	
	cmd[i++] = 0xEF;  
	if (debugLevel > 2) 
	   displayString("CanonVCC1: CMD", cmd, i);
	n = writen(portp->getFileDesc(), cmd, i);
	n = readFromCanon(buf);
	i = 0;
	while ((n<=0) && (i<6))
	{
		Sleep(200);
		n = readFromCanon(buf);
		i++;
	}
    if ((n>0) && (debugLevel > 2))
	    displayString("CanonVCC1: REPLY", buf, n);
	if (debugLevel > 2)
	{
	   printf("CanonVCC1::transmit--readFromCanon ret'd %d: avail = %d\n",
		n,avail);
	}
	if ((n < 0) && (debugLevel > 2))
	   printf("CanonVCC1--ERROR sent by device\n");
	setAvail(n);
	return n;
}

 
int CanonVCC1::readFromCanon(unsigned char *buf)
{
	unsigned char c='\0';
	int n,i=0,x=0; 
	HANDLE fd = portp->getFileDesc();

	/* Read to get the first byte; then read until we get the
	   terminator byte, EFh (or until we tried 10 times).
	   readn() returns -1 (error), 0 (no data arrived),
	   or the number of bytes read (1). */
	n = readn(fd, &c, 1);
	if (n)
	{
		buf[i++] = c;
		while ((c != 0xEF) && (x < 6))
		{
			n = readn(fd, &c, 1);
			if (n)
				buf[i++] = c;
			else if (n==0)
				x++;
			else
			{	
				setAvail(n);
				return n;
			}
		} 
		setAvail(n);
		return i;
	}
	return n;
}


int CanonVCC1::convert(const char *dof, float limit)
{
    float x;
    if (!strcmp(dof, "pan"))
    {
        x = pposn / limit * (float)650.0;
        x += (float)650.0;
        if (x > 1300.0)
            x = 1300.0;
        else if (x < 0.0)
            x = 0.0;
    }  
    else if (!strcmp(dof, "tilt"))
    {
        x = tposn / limit * (float)289.0;
        x += (float)289.0;
        if (x > 578.0)
            x = 578.0;
        else if (x < 0.0)
            x = 0.0;
    }
    else if (!strcmp(dof, "zoom"))
    {
        x = (zposn - (float)1.0) / (float)11.0 * limit;
        if (x < 0.0)
           x = 0.0;
        else if (x > limit)
           x = limit;
    }
    return doubleToInt((double)x);
}

void CanonVCC1::getDesc(char *buf)
{
    char *temp = new char[100];
    strcpy(buf, "\"Canon VC-C1\" ");
    if (!avail)
    {
		strcat(buf, "NOT_RESPONDING");
		delete [] temp;
		return;
    }

    strcat(buf, "pan ");
    sprintf(temp,"%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f",pposn,minpan,maxpan,pspeed,min_pspeed,max_pspeed);
    strcat(buf, temp);
    strcat(buf, "@tilt ");
    sprintf(temp,"%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f",tposn,mintilt,maxtilt,tspeed,min_tspeed,max_tspeed);
    strcat(buf,temp);
    strcat(buf, "@zoom ");
    sprintf(temp,"%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f",zposn,minzoom,maxzoom,zspeed,min_zspeed,max_zspeed);
    strcat(buf,temp);
	delete [] temp;
}
 
