/* 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: canonvcc3.cpp,v 0.4.2 98/08/10 10:28:31 mperry Exp $ (LBL)
*/

#include "canonvcc3.h"

CanonVCC3::CanonVCC3(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-C3\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-C3\n");
		return;
	}
	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 PORT FOR THE CANON VC-C3\n");
			return;
	    }	
	}

	x = setCntlMode("00");
	if (debugLevel > 2)
	   printf("Canon::Canon--setCntlMode ret'd %d; avail = %d\n",x,avail);
	if (!avail)
	{
	    if (debugLevel > 0)
	       fprintf(stderr,"INITIALIZATION FAILED FOR CANON VC-C3\n");	
	    return;
	}
	initialized = 1;
	if (devnum == 1)
	{
	    minpan = -90.0;
	    maxpan = 90.0;
	    mintilt = -30.0;
	    maxtilt = 25.0; 
	    min_pspeed = 1.0;
	    max_pspeed = 76.0;
	    min_tspeed = 1.0;
	    max_tspeed = 70.0;
	    min_zspeed = 0.0;
	    max_zspeed = 7.0;
	    minzoom = 1.0;
	    maxzoom = 12.0;
	    setZoomLimit();
	}
    if (lens)
    {
        pcoeff = (float)72.529;
	    pexp = (float)-0.095;
	    tcoeff = (float)55.423;
	    texp = (float)-0.144;
	    zcoeffw = (float)13.21;
	    zcoeffh = (float)10.177;
	    zexpw = (float)-0.18; 
	    zexph = (float)-0.211;	
	}
	else
	{
	    if (debugLevel > 0)
	    {
	       fprintf(stderr, "WARNING: You did not specify a wide-angle ");
	       fprintf(stderr, "lens for the Canon VC-C3, and it's only ");
	       fprintf(stderr, "calibrated for a wide-angle lens."); 
	    }		
	}
	pspeed = max_pspeed;
	tspeed = max_tspeed;
	zspeed = min_zspeed;
	doZoomSpeedCmd();
}

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

int CanonVCC3::readZoomSpeed()
{
	char req[20]; 
	int x;
	strcpy(req, "0501120401");
	x = transmit((unsigned char *)req, 10);
	return x;
}


int CanonVCC3::setCntlMode(const char *mode)
{
	char req[20]; 
	int x;

	strcpy(req, "05081701");
	strcat(req, mode);
	x = transmit((unsigned char *)req, (int)strlen(req));
	if (debugLevel > 2)
	   printf("Canon::setCntlMode--transmit returned %d\n",x);
	return x;
}


int CanonVCC3::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;
    printf("Canon::move--cmd: %s\n", cmd);
    i = parseCmd(cmd, cmds, &pt, &pspd, &tspd, &zspd);	
    if (pspd || tspd)
        n = doPtSpeedCmd();
    if (zspd)
	n = doZoomSpeedCmd();	

    // Walk array and carry out requested commands in order of occurrence.
    for (j=0; j<i; j++)
    {
        if(pt&&(!strcmp(cmds[j].axis,"pan")||!strcmp(cmds[j].axis,"tilt")))
	{
	    n = doPanAndTilt(); 
	    pt = 0;
	}
        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-C3 (devnum: %d) NOT RESPONDING...CHECK CONNECTION/POWER\n",devnum); 
    }	
    return n;
}


void CanonVCC3::setZoomLimit()
{
    char cmd[12];
    int x,y;

    strcpy(cmd, "0501120203");
    x = transmit((unsigned char *)cmd, 10); 
    x = cmd[5];
    y = cmd[6];	
    x <<= 8; 	
    zoom_limit = (float)(x | y); 	
}


int CanonVCC3::doPtSpeedCmd()
{
        int x;
        char req[40];
 
        strcpy(req, "0705120302");
        x = (int)pspeed;
        intToString(x,2,&req[10]);
        x = (int)tspeed;
        intToString(x,2,&req[12]);
        x = transmit((unsigned char *)req, (int)strlen(req));
        return x;
}


int CanonVCC3::doZoomSpeedCmd()
{ 
        int x;
        char req[40];

        strcpy(req, "0601120402");
        x = (int)zspeed;
        intToString(x,2,&req[10]);
		x = transmit((unsigned char *)req, (int)strlen(req));
        return x;
}
	

int CanonVCC3::doPanAndTilt()
{
        char req[20];
        int x;
        strcpy(req, "0905120502");
        x = convert("pan", maxpan);
        intToString(x, 4, &req[10]);
	    x = convert("tilt", maxtilt);
        intToString(x, 4, &req[14]);
		x = transmit((unsigned char *)req, (int)strlen(req));
        return x;
}


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

    strcpy(req, "0701120202");
    x = convert("zoom", maxzoom);
    intToString(x, 4, &req[10]);
	x = transmit((unsigned char *)req, (int)strlen(req));
    return x;
}


int CanonVCC3::goHome()
{
	int x;
	char req[40]; 

	// Move the Pan/Tilter to home position.
	strcpy(req, "030511");
	x = transmit((unsigned char*)req, 6);
	// Zoom to home zoom position.
	zposn = homezoom;
	doZoom();
	return x;
}


int CanonVCC3::shutDown()
{
	int x = 1;
	/* There is no explicit powerOff command. The SoftwareReset cmd
	   turns off the power but then turns it on again and resets
	   the control mode.  */ 

	//x = doSoftwareReset();
	return x;	
}

int CanonVCC3::doPower()
{
	int x = doSoftwareReset();
	return x;
}

int CanonVCC3::doSoftwareReset()
{
	int x = 0;
	char req[40]; 
	strcpy(req, "030801");
	x = transmit((unsigned char *)req, 6);
	return x;
}


int CanonVCC3::transmit(unsigned char *req, int len)
{
	int x,n;

	// Calculate the checksum, write request and read reply until 
	// we get a response.

    x = calculateChecksum(req, len); 

	if (debugLevel > 2) 
	   displayString("Canon CMD", req, x);
	n = writen(portp->getFileDesc(), req, x);	
	n = getReply(req);
	if (debugLevel > 2)
	{
	   printf("Canon::transmit--first getReply ret'd %d: avail = %d\n",
		n,avail);
	}
	if ((n < 0) && (debugLevel > 2))
	   printf("Canon VC-C3 received NACK/ERROR\n");
	setAvail(n);
	while (avail && (n!=2))
	   n = getReply(req);
	return n;
}

 
int CanonVCC3::getReply(unsigned char *buf)
{
	int x=0, n=0;

    /* Get ACK/NACK or response from the camera and return a
       code indicating type of reply:
            -1 => NACK/error; 0 => nothing read; 1 => ACK; 2 => response
	        3 => notification
       ACK/NACKs are always 4 bytes (byte0 = frame length = 3), and
       bit 7 of frame id (byte1) = 1.
       In an ACK/NACK frame, byte2 indicates whether ok or error--
       if byte2 == 0 => ACK (ok), else, NACK (error).

       Responses/notifications are of variable length; bit 7 of frame id
       (byte1) = 0.  Frame id is used to distinguish between
       ACKS/NACKS and responses/notifications.  To differentiate
       between responses and notifications, look at CID (cmd id)--
       bit5 = 0 for response; bit5=1 for notification.

       Try to read up to a maximum of 3 times.  */

	while ((x==0) && (n<3))
	{
	    x = readFromCanon(buf);
	    if (debugLevel > 2)
	       printf("Canon::getReply--readFromCanon ret'd x: %d\n",x);
	    ++n;
	}	

	// If we haven't read anything, set device to unavailable.
	setAvail(x);
	if (debugLevel > 2)
	{ 
	    printf("Canon::getReply--after readFromCanon--x: %d; avail: %d\n",
		x,avail);
	}

	/* If we read something, set device to available, and
	   return a code indicating the type of reply we received
	   (1 = ACK; -1 = NACK; 2 = RESPONSE; 3 = NOTIFICATION). 
	   Since we are returning in buf the actual reply, the
	   caller can detect/handle errors.  

	   Send an ACK if we receive a response or notification.  */

	if (x > 0)
	{
	    if ((x==4) && (buf[1] > 127))
	    {
		if (buf[2] == 0)
		    x = 1;
	        else 
		    x = -1;
	    }
	    else if ((x>3) && (x<16) && ((buf[1] & 128) == 0))
	    {
		unsigned char ack[4];
	        formatACK(buf,ack);
		writen(portp->getFileDesc(), ack, 4);
		if ((buf[2] & 32) == 0)
		     x = 2;
		else if ((buf[2] & 32) == 32)
		     x = 3;	
	    }		 
	    else
		return -1;	// got garbage or error
        }	
	return x;
}

int CanonVCC3::readFromCanon(unsigned char *buf)
{
 	HANDLE fd = portp->getFileDesc();
	int n=0, x=0;

	// Read 1 byte giving the frame length. Then read "frame length" 
	// number of bytes.  

	buf[0] = 0;
	n = readn(fd, buf, 1);
	if ((int)buf[0] > 0)
	{
	    n = readn(fd, &buf[1], (int)buf[0]);
	    if (debugLevel > 2)
	       displayString("Canon VC-C3 reply",buf,(int)buf[0]);
	    return (n+1);
	} 
	return n;
}


int CanonVCC3::convert(const char *dof, float limit)
{
    float x;
    if (!strcmp(dof, "pan"))
    {
        x = pposn / limit * (float)782.0;
		x *= -1;
        if (x > 782.0)
            x = 782.0;
        else if (x < -782.0)
            x = -782.0;
        x += (float)32768.0;
    }  
    else if (!strcmp(dof, "tilt"))
    {
        if (tposn < 0)
            x = tposn / (float)30.0 * (float)266.0;
        else
            x = tposn / (float)25.0 * (float)217.0;
        if (x > 217.0)
            x = 217.0;
        else if (x < -266.0)
            x = -266.0;
        x += (float)32768.0;
    }
    else if (!strcmp(dof, "zoom"))
    {
        x = (zposn - (float)1.0) / (float)11.0 * zoom_limit;
        if (x < 0.0)
           x = 0.0;
        else if (x > zoom_limit)
           x = zoom_limit;
    }
    return doubleToInt((double)x);
}

int CanonVCC3::calculateChecksum(unsigned char *buf, int len)
{
    int i, x, sum=0;

    x = asciiToPackedHex(buf, len);
    for (i=0; i<x; i++)
		sum += buf[i];	
    for (i=1; (256*i < sum); i++);
    buf[x] = 256 * i - sum;	
    return (x+1);	
}


void CanonVCC3::formatACK(unsigned char *response, unsigned char *ack)
{
    ack[0] = 3;
    ack[1] = 128 + response[1];
    ack[2] = 0;
    ack[3] = 256 - (ack[0] + ack[1] + ack[2]);
}

void CanonVCC3::getDesc(char *buf)
{
    char *temp = new char[100];
    strcpy(buf, "\"Canon VC-C3\" ");
    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;
}
 
